The  Navigation  Potential  of  Ground  Feature 

Tracking 


THESIS 

Giiner  Mutlu,  Captain,  TUAF 
AFIT/GE/ENG/09-52 


DEPARTMENT  OF  THE  AIR  FORCE 
AIR  UNIVERSITY 

AIR  FORCE  INSTITUTE  OF  TECHNOLOGY 

Wright-Patterson  Air  Force  Base,  Ohio 


APPROVED  FOR  PUBLIC  RELEASE;  DISTRIBUTION  UNLIMITED. 


The  views  expressed  in  this  thesis  are  those  of  the  author  and  do  not  reflect  the 
official  policy  or  position  of  the  United  States  Air  Force,  Department  of  Defense,  or 
the  United  States  Government. 


AFIT/GE/ENG/09-52 


The  Navigation  Potential  of  Ground  Feature 

Tracking 


THESIS 


Presented  to  the  Faculty 

Department  of  Electrical  and  Computer  Engineering 
Graduate  School  of  Engineering  and  Management 
Air  Force  Institute  of  Technology 
Air  University 

Air  Education  and  Training  Command 
In  Partial  Fulhllment  of  the  Requirements  for  the 
Degree  of  Master  of  Science  (  Electrical  Engineering  ) 


Giiner  Mutlu,  B.S. 
Captain,  TUAF 


September  2009 


APPROVED  FOR  PUBLIC  RELEASE;  DISTRIBUTION  UNLIMITED. 


AFIT/GE/ENG/09-52 


The  Navigation  Potential  of  Ground  Feature 

Tracking 


Giiner  Mutlu,  B.S. 
Gaptain,  TUAF 


Approved: 


/signed/  Aug  2009 

Dr.Meir  Pachter,  PhD  (Ghairman)  date 

/signed/  Aug  2009 

Dr.  David  Jacques  (Member)  date 

/signed/  Aug  2009 


Lt.Gol  Michael  Veth  (Member) 


date 


AFIT/GE/ENG/09-52 


Abstract 

This  research  effort  examines  the  reduction  of  error  in  inertial  navigation  aided 
by  vision.  This  is  part  of  an  effort  focused  on  navigation  in  a  GPS  denied  environment. 
The  navigation  concept  examined  here  consists  of  two  main  steps.  First,  extract  the 
position  of  a  tracked  ground  object  using  vision  and  geo-locate  it  in  3  dimensional 
navigation  frame.  In  this  hrst  step  multiple  positions  of  the  UAV  are  assumed  known; 
think  of  a  synthetic  aperture.  The  only  information  about  the  tracked  ground  object¬ 
s/features  is  the  unit  vector  that  points  to  the  objects  from  the  center  of  the  camera. 
Two  such  vectors  give  enough  information  to  calculate  the  best  estimate  of  the  po¬ 
sition  of  the  tracked  object  in  a  3  dimensional  navigation  frame  using  the  method 
of  least  square.  Goncerning  the  second  step:  checking  observability  for  the  3-D  case 
shows  that  at  least  2  objects  need  to  be  tracked.  In  practice  one  needs  to  track  more 
than  two  objects  to  wash  out  the  measurement  error  and  obtain  good  results. 

In  the  hrst  step  the  known  position  of  the  UAV  is  used  to  hrst  geo-locate  the  tracked 
ground  object  while  relaying  on  the  short  time  span  accuracy  of  the  INS.  In  the  second 
step,  the  information  on  the  position  of  multiple  tracked  objects  is  used  as  points  of 
origin  of  vectors  that  point  to  the  UAV,  instead  of  using  multiple  positions  of  the  air 
vehicle,  as  was  the  done  in  step  one.  The  least  squares  method  is  applied  to  determine 
the  position  of  the  UAV.  As  explained  above,  initially  using  known  positions,  of  the 
air  vehicle,  the  positions  of  the  tracked  ground  objects  are  calculated,  and  then  using 
those  calculated  positions  the  position  of  the  UAV  is  determined.  These  two  steps 
feed  each  other. 

The  performance  of  the  Kalman  hlter  and  UAV  position  estimation  algorithms  strongly 
depend  on  the  quality  of  the  measurements  provided  by  the  INS.  The  INS  measure- 
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ments  were  modeled,  and  we  investigated  simplified  fiying  scenarios  to  validate  our 
algorithms  /  methodology. 

Some  effort  was  expanded  on  investigating  the  possibility  of  using  a  camera  and 
GPS  position  measurements, without  using  INS.  Directly  using  the  Doppler  readings 
provided  by  GPS  might  be  advantageous. 

The  investigated  scenarios  showed  that  bearings  measurements  of  ground  objects 
taken  over  time  are  useful  for  enhancing  the  INS  navigation  solution.  When  multiple 
objects  are  tracked,  the  algorithm  is  applied  to  each  pair  of  objects  and  the  results 
were  averaged.  This  method  gave  us  better  results  and  at  the  same  time  is  somewhat 
more  practical  than  extending  the  algorithm  to  accommodate  an  arbitrary  number  of 
tracked  objects. 
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The  Navigation  Potential  of  Ground  Feature 

Tracking 

I.  Introduction 

1 . 1  Background 

Navigation  has  a  long  history.  Currently,  commercial  nav-aids  like  VOR  (VHF 
Omni-directional  Radio  Range),  NDB  (Non-directional  beacon),  DME  (Distance  mea¬ 
suring  equipment)  work  well  and  GPS  (Global  positioning  system)  provides  very  pre¬ 
cise  positioning,  and  it  is  widely  used.  In  this  thesis  the  use  of  vision  for  navigation 
is  investigated  for  use  in  areas  of  conflict  with  perhaps  reduced  GPS  availability,  in 
order  to  guarantee  more  robust  navigation. 

Consider  a  target  located  thousands  of  miles  away  from  the  nearest  safe  base 
in  a  partner  country;  and  assume  the  enemy  is  surrounded  by  neighboring  countries 
which  are  not  willing  to  provide  logistic  support  to  our  operation.  An  UAV  would 
be  used  in  such  an  operation  because  not  using  personnel  will  save  lives.  The  UAV 
should  be  able  to  take  part  in  a  global  operation.  In  addition,  a  small  UAV  will  be 
cheaper  than  a  conventional  jet  hghter  or  bomber,  and  there  is  no  need  to  use  an  Air 
Force  installation  close  to  the  area  of  conflict.  The  operation  can  be  started  sooner 
and  will  cost  less. 

How  can  this  be  accomplished?  The  UAV  can  be  launched  safely  from  one  of 
our  Air  Bases,  and  can  fly  on  its  route  using  conventional  nav-aids  without  having 
any  problems  until  approaching  the  area  of  conflict.  In  the  battle  zone  it  needs  to 
be  prepared  to  lose  some  nav-aid  options.  Autonomous  vision-aided  INS  navigation 
will  be  helpful  here.  This  is  a  passive  navigation  method  that  cannot  be  jammed  by 
electromagnetic  interference.  Also,  an  IR  camera  could  be  used  since  it  is  less  affected 
by  humidity  in  the  air  or  cloud.  In  the  battle  space,  say  an  area  of  a  hundred  square 
miles,  navigation  will  be  accomplished  by  tracking  ground  features  using  an  airborne 
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camera. 


Furthermore,  stationary  ground  objects,  like  trees,  big  rocks,  buildings  or  other 
objects  will  be  used  to  map  the  battle-zone.  Our  hunter  UAV  is  a  moving  object,  and 
other  moving  objects  are  possibly  friends  or  enemies.  The  object  tracking  algorithm 
will  calculate  the  relative  position  between  stationary  objects,  the  UAV  and  the  target, 
and  possibly  other  moving  objects.  Kalman  hltering  will  be  used. 

1.2  Problem  Statement  for  Level  Flight  and  Maneuvers 

During  level  flight  the  ground  object  should  be  continuously  tracked.  Using  the 
known  speed  and  altitude  of  the  UAV  the  position  of  the  tracked  object  in  the  next 
image  can  be  predicted.  This  will  narrow  the  search  for  locating  the  object /feature 
in  the  next  frame.  To  not  lose  the  update  information,  it’s  necessary  to  track  four 
or  hve  ground  objects.  It  all  depends  on  how  good  the  real-time  feature  extraction 
algorithm  is. 

We  note  that  tracked  objects  in  the  scene  should  not  be  arranged  on  one  line. 
It’s  good  to  have  the  tracked  objects  spread  out  in  the  scene  in  every  frame.  The 
overlap  of  the  objects’  pattern  in  the  scene  will  provide  continuous  update  information 
and  will  be  a  good  aid  to  INS. 

Unfortunately  flights,  especially  military  flights,  will  not  be  only  level  flights. 
Military  aircraft  and  even  UAVs  need  to  maneuver.  Let’s  take  a  barrel  roll  as  an 
example.  A  down  looking  camera  will  lose  sight  of  the  ground  objects  for  a  short  time 
during  upside-down  flight.  One  should  then  map  the  last  group  of  visible  ground  ob¬ 
jects  on  a  digital  map  of  the  3  dimensional  world  around  the  UAV,  and  then  calculate 
the  predicted  position  of  those  objects  until  the  time  of  recapturing  of  at  least  one 
ground  object.  Hence,  to  not  lose  the  tracked  objects  a  camera  which  has  a  wide 
angle  lens  such  that  it  can  capture  a  wide  held  of  view  is  required. 

In  the  case  of  a  total  loss  of  ground  objects,  forward  propagation  of  the  UAV’s 
position  might  not  be  reliable.  If  we  have  a  good  GPS  signal  or  update  from  other 
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sensors,  sensor  fusion  will  be  helpful  here  to  aid  the  INS.  Especially  for  this  case, 
mapping  of  the  tracked  objects  in  the  3D  world  and  saving  them  in  memory  is  very 
important;  the  combat  zone  could  be  an  area  less  than  100  square  miles  and  informa¬ 
tion  about  many  ground  objects  could  be  stored  in  memory.  Good  3  —  D  maps  of  that 
area  can  be  extracted  and  stored  in  memory  like  pieces  of  a  huge  puzzle.  Flying  a  few 
times  over  the  same  area  may  help  to  £11  out  some  gaps.  It  might  not  be  imperative 
to  fill  all  gaps  in  the  battle  space.  We  will  need  navigation  information  to  get  the  job 
done  and  finally  to  exit  the  battle  zone  and  return  to  base.  Producing  that  kind  of  3D 
map  by  filling  gaps  depends  on  really  good  feature  tracking  and  good  matching/image 
correspondence  algorithms. 

1 . 3  Scope 

There  is  increased  use  of  UAVs  today.  What  requires  increased  numbers  of 
sorties  of  UAVs  in  the  battlefield?  Increasing  sensitivity  of  the  public  to  lost  personnel 
force  leadership  to  look  for  methods  to  save  life  of  troops.  Using  unmanned  systems 
reduces  risk  to  the  crew. 

There  is  always  a  need  to  reduce  the  cost.  The  cost  of  production  and  also  the  cost 
of  maintenance.  If  you  don’t  need  to  have  a  pilot  in  the  cockpit,  that  means  you 
will  save  weight  and  also  here  will  be  no  need  for  systems  designed  to  save  his  life. 
Here  we  can  include  input-output  devices  that  help  the  pilot  to  communicate  with 
the  aircraft,  and  to  control  it.  Many  subsystems  are  needed  for  a  human  on  board. 
Using  an  UAV  will  save  weight  and  expenses  related  to  those  devices.  Affordable  war 
machines  are  always  attractive  to  the  leadership. 

We  need  to  add  that  pilot  emergencies  are  not  expected  in  UAVs.  Are  they  eligible 
to  replace  all  jet  fighters  or  bombers?  For  now  there  is  still  need  to  use  conventional 
weapon  systems,  however  a  lot  of  studies  are  done  to  improve  the  capabilities  of 
unmanned  systems  in  the  air,  on  the  ground  and  at  sea. 

What  about  requirement  for  invisibility  in  the  battlefield?  There  are  many  different 
sensors  available  for  navigation.  Radar,  ladar,  laser  altemeter  etc...  We  need  to  be 
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able  to  use  all  options,  even  that  we  will  not  use  all  of  them  at  the  same  time  or  at 
the  same  aircraft.  Emitting  sensors  could  be  dehned  by  the  enemy,  so  that  requires 
research  to  use  passive  sensors  for  navigation  for  some  cases. 

In  this  thesis  the  use  of  vision  for  navigation  is  investigated  for  use  in  areas  of  conflict 
with  perhaps  reduced  GPS  availability,  in  order  to  guarantee  more  robust  navigation. 
Vision  aid  is  passive  and  could  be  fully  autonomous.  Does  not  need  help  from  satellite 
or  other  sources.  It  is  not  electronically  jammable.  Can  replace  some  of  other  INS 
aid  sensors. 

1.4  Approach/ Methodology 

To  document  the  research  conducted  in  this  thesis,  each  chapter  will  be  briefly 
summarized.  The  first  chapter  some  background  information  abut  navigation,  prob¬ 
lem  statement  from  hight  point  of  view,  brief  information  about  Systems  Engineering 
and  Management  and  its  principles,  and  finally  summary  of  the  whole  document. 
The  second  chapter  includes  background  information  about  INS  and  different  types 
of  frames  used  in  navigation.  Chapter  three  examines  mathematical  theory  used  in 
this  thesis.  This  chapter  Erst  investigates  2-D  flight  scenario  and  related  mathematics; 
then  extends  the  theory  and  mathematics  for  3-D  flight  scenario.  Methods  to  go  be¬ 
tween  frames  are  explained  and  applied  for  investigated  flight  scenario  in  this  chapter. 
Time  dependent  state  space  representation  needed  for  Kalman  filtering  was  obtained 
in  this  section.  Chapter  four  consists  of  simulations,  results,  and  briefly  information 
about  assumptions  for  applied  scenarios.  Chapter  five  consists  of  conclusion  of  the 
job  done  here  and  recommendations  for  next  iterations. 
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II.  Literature  Review 


2. 1  Inertial  Navigation 

The  inertial  navigation  systems  depend  on  mechanic  laws  derived  by  Newton. 
An  inertial  navigation  system  consist  of  a  computer  and  a  platform  or  module  con¬ 
taining  accelerometers  and  gyroscopes.  Inclinometers  are  also  sometimes  used.  The 
INS  is  initially  provided  with  its  position  and  velocity  from  another  source  (a  human 
operator,  a  GPS  receiver,  etc.),  and  thereafter  computes  its  own  updated  position  and 
velocity  by  integrating  information  received  from  the  inertial  sensors.  The  advantage 
of  an  INS  is  that  it  requires  no  external  information  in  order  to  determine  its  position, 
orientation,  or  velocity  once  it  has  been  initialized. 

An  INS  can  autonomously  detect  a  change  in  its  geographic  position  (a  move 
East  or  North,  for  example),  a  change  in  its  velocity  (speed  and  direction  of  move¬ 
ment),  and  a  change  in  its  orientation  (rotation  about  an  axis).  It  does  this  by 
measuring  the  linear  and  angular  accelerations  of  the  UAV.  Since  it  requires  no  exter¬ 
nal  reference  (after  initialization),  it  is  immune  to  jamming  and  deception.  Inertial- 
navigation  systems  are  used  in  ground  vehicles,  aircraft,  ships,  submarines,  and  guided 
missiles. 

Gyroscopes  measure  the  angular  velocity  of  the  system  in  the  inertial  reference 
frame.  By  using  the  original  orientation  of  the  aircraft  in  the  inertial  reference  frame 
as  the  initial  condition  and  integrating  the  angular  velocity,  the  system’s  current  ori¬ 
entation  is  obtained  at  all  times.  This  can  be  thought  of  as  the  ability  of  a  blindfolded 
passenger  in  a  car  to  feel  the  car  turn  left  and  right  or  tilt  up  and  down  as  the  car 
ascends  or  descends  hills.  Based  on  this  information  alone,  he  knows  in  what  direction 
the  car  is  leading  but  does  not  know  how  fast  or  slow  it  is  moving,  or  whether  it  is 
sliding  sideways. 

Accelerometers  measure  the  linear  acceleration  of  the  system  in  the  inertial 
reference  frame,  but  in  directions  that  can  only  be  measured  relative  to  the  moving 
system  (since  the  accelerometers  are  affixed  to  the  vehicle  and  rotate  with  the  vehicle, 
but  are  not  aware  of  their  own  orientation).  This  can  be  thought  of  as  the  ability  of  a 
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blindfolded  passenger  in  a  car  to  feel  himself  pressed  back  into  his  seat  as  the  vehicle 
accelerates  forward  or  pulled  forward  as  it  slows  down;  and  feel  himself  pressed  down 
into  his  seat  as  the  vehicle  speeds  up  a  hill  or  rise  up  out  of  his  seat  as  the  car  passes 
over  the  crest  of  a  hill  and  begins  to  descend.  Based  on  this  information  alone,  he 
knows  how  the  vehicle  is  moving  relative  to  itself,  that  is,  whether  it  is  going  forward, 
backward,  left,  right,  up  (toward  the  car’s  ceiling),  or  down  (toward  the  car’s  floor) 
measured  relative  to  the  car,  but  not  the  direction  relative  to  Earth,  since  he  did  not 
know  what  direction  the  car  was  facing  relative  to  Earth  when  he  felt  the  accelera¬ 
tions. 

All  inertial  navigation  systems  suffer  from  “integration  drift”:  small  errors  in 
the  measurement  of  acceleration  and  angular  velocity  are  integrated  into  progressively 
larger  errors  in  velocity,  which  is  compounded  into  still  greater  errors  in  position. 
Since  the  new  position  is  calculated  solely  from  the  previous  position,  these  errors 
are  cumulative,  increasing  at  a  rate  roughly  proportional  or,  on  even  higher  route  to 
the  time  since  the  initial  position  was  input.  Therefore  the  INS  position  fix  must  be 
periodically  corrected  by  input  from  some  other  type  of  navigation  system. 
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2.2  Coordinate  Systems 

2.2.1  The  Inertial  reference  frame.  The  location  of  the  origin  may  be  any 
reference  point  that  is  completely  unaccelerated  (inertial),  and  the  orientation  of  the 
axes  is  usually  irrelevant  in  most  problems  so  long  as  they  too  are  hxed  with  respect 
to  inertial  space.  The  origin  of  the  inertial  reference  frame  I  is  at  the  center  of  the 
Earth.  The  geometry  is  illustrated  in  Figure2.1 


z 


Figure  2.1:  The  Inertial  reference  frame 
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2.2.2  The  Earth- Centered  reference  frame.  As  its  name  suggests  this  coor¬ 
dinate  system  has  its  origin  at  the  center  of  the  Earth.  Its  axes  may  be  arbitrarily 
selected  with  respect  to  hxed  positions  on  the  surface  of  the  Earth. This  ECEF  co¬ 
ordinate  system  obviously  rotates  with  the  Earth.  The  geometry  is  illustrated  in 


Eqiiatoi’ 


Zero 
Longitude 


Figure  2.2:  Earth-Centered  Earth-Fixed  reference  frame  ECEF 


2.2.3  The  Earth-fixed  reference  frame.  This  coordinate  system  has  its  origin 
fixed  to  an  arbitrary  point  on  the  surface  of  the  Earth,  xe  points  due  North,  ue  points 
due  East,  and  ze  points  toward  the  center  of  the  Earth. 


Figure  2.3:  Earth-fixed  reference  frame 

2.2.4  The  Body-fixed  frame.  This  coordinate  system  has  its  origin  fixed  to 
any  arbitrary  point  that  may  be  free  to  move  relative  to  the  Earth.  For  example,  the 
origin  may  be  fixed  to  the  center  of  gravity  (CG)  of  an  aircraft  and  move  with  the 
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CG.  i/AT  points  due  North,  xe  points  due  East,  and  ze  points  toward  the  center  of  the 
Earth. 


Figure  2.4:  Body-hxed  navigation  reference  frame 

2.2.5  Body- fixed  reference  frames.  “Body-hxed”  means  the  origin  and  axes 
of  the  coordinate  system  are  hxed  with  respect  to  the  (nominal)  geometry  of  the 
aircraft.  This  must  be  distinguished  from  body-carried  systems  in  which  the  origin  is 
hxed  with  respect  to  the  body  but  the  axes  are  free  to  rotate  relative  to  it.  In  hight 
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dynamics  reference  frames  are  used  which  have  their  origin  at  the  aircraft’s  CG  and 
the  X  axis  is  aligned  with  the  velocity  vector  and  one  then  refers  to  the  “wind  axes” . 
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III.  Methodology 


3. 1  Introduction 

In  this  chapter  we  develop  methods  for  visual  INS  aiding  using  optical  measure¬ 
ments  of  tracked  ground  objects/ground  features  with  known  or  unknown  position. 
INS  aiding  using  bearing  measurements  of  stationary  ground  features  is  investigated. 
The  objective  is  to  quantify  the  navigation  information  obtained  by  tracking  ground 
features  over  time.  The  answer  is  provided  by  an  analysis  of  the  attendant  observabil¬ 
ity  problem.  The  degree  of  INS  aiding  action  is  determined  by  the  degree  of  observ¬ 
ability  provided  by  the  measurement  arrangement.  The  latter  is  strongly  influenced 
by  the  nature  of  the  available  measurements  -  in  our  case,  bearing  measurements 
of  stationary  ground  objects  -  the  trajectory  of  the  aircraft,  and  the  length  of  the 
measurement  interval.  It  is  shown  that  when  one  known  ground  object  is  tracked, 
the  observability  Grammian  is  rank  dehcient  and  thus  full  INS  aiding  action  is  not 
available.  However,  if  barometer  altitude  is  available  and  an  additional  vertical  gyro¬ 
scope  is  used  to  provide  an  independent  measurement  of  the  aircraft’s  pitch  angle,  a 
data  driven  estimate  of  the  complete  navigation  state  can  be  obtained.  If  two  ground 
features  are  simultaneously  tracked  the  observability  Grammian  is  full  rank  and  all 
the  components  of  the  navigation  state  vector  are  positively  impacted  by  the  external 
measurements. 

3.2  2-D  Case 

3.2.1  Introduction.  Inertial  Navigation  System  aiding  using  optical  mea¬ 
surements  [1-6]  is  appealing  because  passive  bearing  -  only  measurements  preserve 
the  autonomy  of  the  integrated  navigation  system.  In  this  thesis  an  attempt  is  made 
to  gain  an  understanding  of  the  nature  of  the  navigation  information  provided  by 
bearing  -  only  measurements  taken  over  time,  of  stationary  ground  objects,  whose 
position  is  not  necessarily  known. 
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In  this  thesis  the  crucial  issue  of  processing  the  images  provided  by  a  down  look¬ 
ing  camera  for  the  autonomous  -  without  human  intervention  -  measurement  of  optic 
flow,  or,  alternatively,  image  correspondence  for  feature  tracking,  or,  mosaicing,  [7, 8] 
is  not  addressed.  We  do  however  note  that  these  tasks  are  nevertheless  somewhat 
easier  than  full  blown  Autonomous  Target  Recognition  (ATR)  and/or  machine  per¬ 
ception.  In  this  article  it  is  assumed  that  autonomous  feature  detection  and  tracking 
is  possible  so  that  bearings  measurement  records  of  stationary  ground  objects  are 
available.  We  exclusively  focus  on  gauging  the  navigation  potential  of  bearings  -  only 
measurements  of  stationary  ground  objects  taken  over  time. 

In  this  thesis  we  refer  to  purely  deterministic,  i.e.  noise  free,  corrupted  bearing 
measurements,  where  many  measurements  would  naturally  help  wash  out  the  noise. 
Even  so,  both  the  number  of  bearing  measurements  taken  and  the  number  of  simul¬ 
taneously  tracked  ground  objects  is  of  interest.  Obviously,  increasing  the  number  of 
tracked  features  should  increase  the  navigation  information  content.  Thus,  our  main 
thrust  is  focused  on  the  quantihcation  and  measurement  of  the  degree  of  observability 
of  the  optical  bearings-only  measurement  arrangement.  In  this  respect,  we  follow  in 
the  footsteps  of  [9]  and  [10].  The  absolute  minimal  number  of  tracked  features  such 
that  additional  features  do  not  further  increase  the  degree  of  observability,  is  estab¬ 
lished. 

Concerning  observability:  The  measurement  equations  are  time-dependent  and  there¬ 
fore  one  cannot  ascertain  observability  from  an  observability  matrix.  Hence,  the  infor¬ 
mation  content  of  passive  optical  bearings-only  measurements  will  be  gauged  by  deriv¬ 
ing  the  observability  Grammian  of  the  bearing-only  measurement  arrangement.  It  is 
however  very  important  to  hrst  non-dimensionalize  the  navigation  state,  the  bearings 
measurement  geometry,  and  also  the  time  variable,  so  that  the  derived  observabil¬ 
ity  Grammian  is  non-dimensional. This  guarantees  that  the  observability  Grammian 
correctly  reflects  the  geometry  of  the  measurement  arrangement,  so  that  meaningful 
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conclusions  can  be  drawn.  The  rank,  or  the  condition  number  of  the  observability 
Grammian  is  established  -  it  is  the  ultimate  purveyor  of  the  navigation  potential  of 
vision. 

A  word  of  caution  concerning  information  fusion  is  in  order.  It  is  often  much  too 
easy  to  set  up  a  Kalman  Filter  (KF)  for  fusing,  e.g.,  optical  and  inertial  measurements 
-  in  which  case  one  then  refers  to  INS  aiding  using  bearings-only  measurements  [11]. 
The  point  is  that  a  KF  output  will  always  be  available.  Even  in  the  extreme  case  of 
an  optical  measurement  arrangement  where  observability  is  non-existent,  a  KF  can 
be  constructed  and  a  valid  navigation  state  estimate  output  will  be  available,  except 
that  no  aiding  action  is  actually  taking  place:  the  produced  navigation  state  estimate 
then  exclusively  hinges  on  prior  information  only  -  in  this  case,  inertial  measurements, 
whereas  the  optical  measurements  don’t  at  all  come  into  play. 

In  light  of  the  above  discussion,  the  real  question  is  whether  there  is  aiding  ac¬ 
tion  so  that  the  optical  measurements  are  actually  brought  to  bear  on  a  KF  provided 
navigation  state  estimate,  thus  yielding  enhanced  estimation  performance  relative  to 
a  stand  alone  INS.  One  would  like  to  determine  how  strong  the  aiding  action  is  and 
into  precisely  which  components  of  the  navigation  state  the  aiding  action  trickles 
down  into.  The  answer  is  provided  by  our  deterministic  observability  analysis: 


A  KF  will  help  enhance  the  accuracy  of  the  complete  navigation  state  if,  and 
only  if,  the  system  is  observable,  that  is,  the  observability  Grammian  is  full  rank. 
We’ll  also  address  the  case  of  partial  observability. 


Strictly  speaking,  the  herein  outlined  analysis  should  be  carried  out  whenever 
the  use  of  a  KF  is  contemplated,  to  get  a  better  understanding  of  the  data  fusion 
process.  For  example,  it  is  known  that  when  during  cruise,  GPS  position  measure- 
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ments  are  used  to  aid  an  INS,  no  aiding  action  will  be  realized  in  the  critical  aircraft 
heading  navigation  state  variable,  unless  the  aircraft  is  performing  high-g  horizontal 
turns.  And  in  the  extreme  case  of  an  object  in  free  fall  -  for  example,  a  bomb  -  it  can 
be  shown  that  GPS  position  measurements  will  not  enhance  the  estimate  of  any  of 
the  object’s  Euler  angles. 


Naturally,  the  use  of  bearings-only  measurements  can  yield  information  on  an¬ 
gular  components  only  of  an  air  vehicle’s  navigation  state.  We  refer  to  the  measure¬ 
ment  of  the  aircraft  “drift”  angles,  namely,  the  angles  included  between  the  ground 
referenced  velocity  vector  and  the  aircraft  body  axes.  A  dramatic  improvement  in 
the  navigation  state  information  can  be  obtained  if  additional  passive  measurements 
become  available.  A  case  in  point  is  baro-altitude  measurement.  The  latter  is  also 
used  in  inertial  navigation  -  to  the  extent  that  high  precision  inertial  navigation  is 
not  possible  without  passive  baro  altitude,  or,  GPS  -  provided,  altitude  information. 
Indeed,  the  combined  use  of  optical  measurements  and  baro-altitude  for  navigation 
has  a  long  history,  from  the  days  when  navigators  where  seated  in  the  glazed  nose 
of  aircraft  and  would  optically  track  a  ground  feature  using  a  driftmeter,  or,  on  the 
continent,  a  cinemoderivometer. 

We  are  also  interested  in  the  use  of  additional  passive  measurements  and  side 
information.  We  refer  to  known  landmarks,  digital  terrain  elevation  data,  LADAR 
provided  range  measurements,  GPS  provided  position  measurements,  and,  hnally,  the 
mechanization  of  an  autonomous  navigation  system  akin  to  an  INS  which  however  ex¬ 
clusively  and  continuously  uses  bearing  measurements  of  ground  features  -  one  would 
then  refer  to  an  Optical  Navigation  System  (ONS). 

By  augmenting  the  navigation  state  with  the  coordinates  of  the  tracked  ground 
features/objects.  Simultaneous  Location  and  Mapping  (SLAM)  is  achieved  in  a  unihed 
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framework.  Furthermore,  the  analysis  can  be  extended  to  include  the  tracking  of 
moving  objects,  whether  on  the  ground,  or  in  the  air.  Under  standard  kinematic 
assumptions,  the  motion  of  own  ship  relative  to  said  tracked  objects  can  be  measured: 
think  of  obstacle  avoidance  or  “see  and  avoid”  guidance. 

3.2.2  Modeling.  We  are  cognizant  of  the  fact  that  the  degree  of  INS  aiding 
provided  by  bearing  measurements  might  strongly  depend  on  the  trajectory  flown. 
For  example,  this  certainly  is  the  case  when  GPS  position  measurements  are  used  for 
INS  aiding.  All  this  notwithstanding,  in  this  study  we  conhne  our  attention  to  the 
most  basic  two-dimensional  scenario  where  the  aircraft  is  flying  wings  level  at  constant 
altitude  in  the  vertical  plane.  The  two-dimensional  scenario  under  consideration  is 
illustrated  in  Figured.  1. 


In  2-D,  the  navigation  state  is 


and  the  “disturbance” 

d  =  {Sf^,Sf^,Su) 

consists  of  the  biases  Sfx  and  Sfz  of  the  accelerometers  and  the  bias  6uj  of  the  rate 
gyro.  The  error  equations  are 


6X  =  A6X  +  rd  (3.1) 

We  assume  the  Earth  is  flat  and  non-rotating  -  this,  in  view  of  the  short  duration  and 
the  low  speed  of  the  Micro  Air  Vehicle  (MAV)  under  consideration.  Consequently,  in 
level  flight  the  dynamics  are 
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Figure  3.1:  Flight  in  the  vertical  plane. 
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(3.2) 


The  tracked  ground  object  is  located  at  P  =  {xp,Zp).  From  the  2-D  geometry  in 
FigS.l  we  derive  the  measurement  equation: 


Z  -  Zr, 


=  cot(«  +  f) 


(3.3) 


and  thus  the  measurement 


tan(0  = 


z  —  Zp  —  {xp  —  x)  tan  6  f 
Xp  —  X  +  {z  —  Zp)  tan  9  Xf 


(3.4) 


or 


Xp  —  X  +  {z  —  Zp)  tan  9  ^ 
^  z  —  Zp  —  [xp  —  x)  tan  9 


(3.5) 


Now,  the  INS  output  Xc  =  x  +  6x,  Zc  =  z  +  Sz,  9c  =  9  +  69  where  x,  z  and 
9  are  the  true  navigation  states.  In  order  to  linearize  the  measurement  equation,  we 
need  the  information  x~  ,z~,  that  is,  prior  information  on  the  position  of  the  tracked 
ground  feature. 


Let 


Xp  Sxp 

(3.6) 

Zp  +  6Zp 

(3.7) 
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The  true  augmented  state 


X  =  Xc  —  Sx  (3.8) 

z  =  Zc  —  Sz  (3.9) 

Vx  =  Vx,  -  5vx  (3.10) 

Vz=v^^-5v^  (3.11) 

e  =  6^-66  (3.12) 

Xp  =  x~  —  5xp  (3.13) 

Zp  =  z~  -  Szp  (3.14) 


where  6x,  6z,  Svx,  Svz,  S6,  Sxp,  6zp  are  the  augmented  state’s  estimation  errors. 

During  level  flight,  and  using  the  small  angle  approximation,  the  measurement  equa¬ 
tion  is 


X  +  {z  Zp)6  ,  , 

/  z-  Zp-{xp-  x)e 

and  the  measurement,  expressed  as  a  function  of  the  state  perturbations  5x,  Sz,  S9, 6xp,  6zp 
is: 
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(3.16) 


Xf  Xp  -  5xp  -  Xc  +  5x  +  (zc  -  5z  -  Zp  +  5zp)  ■  {6c  -  56) 
f  Zc  —  5z  —  Zp  +  5zp  —  {Xp  —  5xp  —  Xc  +  5x)  ■  (6c  —  56) 


Xp  —Xc+(zc—Zp  )6c+5x—6c5z+(zp  —Zc)56—5xp+6c5zp 
Zc—Zp+(xc—Xp)6c—6c5x—5z+(xp—Xc)56+6c5xp+5zp 


Xp  -Xc+(zc-Zp  )6c  ^ _ 1 _ 

Zc-Zp+(Xc-Xp)6c  (zc-Zp+{Xc-Xp)9cf  ^ 


Zp  +  (Xc 


Xp)6c 


+(Xp  -Xc  +  (zc  -  Zp  )6c)6c]5x 


+  [-(Zc  -  Zp  +  (Xc  -  Xp  )6c)6c  +  Xp  -Xc+  (Zc  -  Zp  )6c]  5z 


+  [(Zc  -Zp  +  (Xc  -  Xp  )6c)(Zp  -  Zc)  +  (Xp  -Xc+  (Zc  -  Zp  )6c)(Xc  -  Xp  )]  56 


+  [Zp  -  Zc+  (Xp  -  Xc)6c  +  (Xc  -Xp  +  (Zp  -  Zc)6c)6c]  5Xp 


+  [(Zc  -  Zp  +  (Xc  -  Xp  )6c)6c  +  Xc-Xp  +  (Zp  -  Zc)6c]  5Zp  } 


X-n  —Xc+(Zc  —  Zp.  )6*c  1  , 

=  -z-+ix  -x-)6  ^  r - ^  “  7  +  -  7)^c] 

Zc  Zp  Xp  )ac  Zc-Zp  +{xc-Xp  )6c 


+  [xp  -  Xc  +  (Xp  -  Xc)6c]  5z-  [{Xp  -  Xc)"^  +  (Zp  -  Zc)"^]  56+  [zp  -  Zc  +  (Zp  -  Zc)6l]  5xp 
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+  [xc  -Xp  +  (Xc  -  Xp  )6f\  SZp  } 


Xp  -Xc+{zc-Zp  )9c 
Zc-Zp  +{xc-Xp)6c 


Zc-Zp  +{xc-Xp  )6c  ‘ 


(1  +  91) {zc  -  Zp  )Sx  +  (1  +  e^^){Xp 


{Xp  -  +  {Zp  -  Zc)"^]  56*  +  (1  +  6l){Zp  -  ZcjSxp  +  (1  +  9'^){xc 


X  )6zp 


Xp  -Xc+{Zc-Zp  )ec  ^ _ 1+^2 _ 

Zc-Zp+{xc-Xp)ec  ^zc-zp+{xc-xp)ecf  ^ 


Zp  )6x  +  {Xp 


Xc)Sz 


-  '{Xp  -  Xc)"^  +  {Zp  -  Zc)^]69  +  {Zp  -  Zc)6xp  +  {xc  -  Xp  )6zp  } 


We  use  the  fact  that  the  navigation  state  errors  are  small  and  therefore  the  for¬ 
mula  ~  1  -f-  a;  is  repeatedly  used  in  derivations  above  to  simplify  the  measurement 
equation  and  to  linearize  it. 

Hence,  for  level  flight  the  linearized  measurement  eqnation  is 

Xf  X--Xc+{Zc-Z-)9c  _  1  r/  -X. 

-? - - = - „  —  - - To  -1  (Zc  —  Z„  )0X 

J  Zc—Zp  +{xc—Xp  )6c  z^—z-j^(^Xc—Xp)6c 


Zr{Xp  -Xc)Sz-[{Xp  -Xc)^  +  {Zp  -Zc)‘^]69  +  {Zp  -Zc)6xp  +  {xc-Xp)6zp}  (5) 


Xc)Sz 
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3.2.3  Special  Cases.  If  the  position  (xp,  Zp)  of  the  tracked  ground  object  is 
known,  the  state  is  the  navigation  state  x,  z,  v^,  Vz,  9  and  the  linearized  measurement 
equation  is 

n  _  Xv-^C+(Zc-Zp)Oc _ 1 _ ^ 

/  [Zc-Zp+(Xc-X.p)Oc\^  '  +  V" 

+  [{Xp  -  Xcf  +  {Zp  -  Zcf]se  } 


If  only  the  elevation  Zp  of  the  tracked  ground  object  is  known,  the  augmented  state 
is  X,  z,Vx,Vz,9,Xp.  Without  loss  of  generality  we  set  Zp  =  0,  and  the  linearized  mea¬ 
surement  equation  is: 


^  -^c+Zgec _ 1  .  A  -n  t  _  U 

/  Zc+{Xc-Xp)9c  [Zc+iXc-Xp)6c\‘^  {  Zc  X  (Xp  Xc)  z 


+[{Xp  -  XcY  +  zl]56  -  ZcSxp  } 


Remark  :  For  the  purpose  of  analysis,  on  the  right  hand  side  of  the  measurement 
equation  one  can  replace  Xc  by  the  true  -  that  is,  the  nominal  -  state  component  x  , 
2:c  by  the  true  -  that  is,  the  nominal  -  state  component  z,  and  6'c  by  6*  =  0. 


Hence,  if  the  position  of  the  tracked  ground  object  is  known,  and,  in  addition,  without 
loss  of  generality  we  set  Zp  =  0  ,  then  the  linearized  measurement  equation  is 


/ 


Zc  (^Xc  Xp^Ofz 


■  {^z5x  +  {xp  —  x)Sz 


(xp  —  xY  +  z^]59^ 


(3.17) 
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If  only  the  elevation  Zp  of  the  tracked  ground  object  is  known,  and,  as  before  without 
loss  of  generality,  we  set  Zp  =  0,  then  the  state  error  is  5x,  6z,  Sv^,  Svz,  69, 6xp,  and  the 
linearized  measurement  equation  is 


Xf  Xp  -  Xc  +  z9c 
f  Zc+{x-  Xp)9c 


-■{z6x  +  { 


x)6z  —  [{Xp  —  xY  +  z‘^]6d  —  z6xp^  (3.18) 


If  the  position  of  the  ground  object  is  not  known,  the  navigation  state  error  is 
5x,  5z,  5vx,  6vz,  66,  6xp,  6zp  and  the  linearized  measurement  equation  -  see,  e.g.,  eq.(5) 
-  is 


X  f  Xp  -Xc+{z-Zp  )6c 

f  z-Zp  +{x-Xp)9c 


z_z-\2  ■{i.z-Zp  )6x  +  [Xp  -  x)6z 

Z  Zp  ) 


-  '.{Xp  -  +  iz-Zp  YYG  +  {z-Zp  )6xp  +  {x-Xp  )6zp  } 


Furthermore,  for  the  purpose  of  analysis,  on  the  right  hand  side  of  the  last  two 
equations  we  can  also  replace  Xp  and  Zp  by  the  true  position  {xp,  Zp)  of  the  tracked 
object.  Hence  the  respective  measurement  equations  are 


Xf  Xp  -  Xc  +  z9c 
f  Zc+{x-  Xp)9c 


—  ■  \^z6x  +  [xp  —  x)6z 


{xp  —  xY  +  z‘^]69  —  z6xp^  (3.19) 


and 

a(/  _  Xp-Xc+{z-Zp)ec  _  1 

/  z-Zp +{x-Xp)9c  {z-ZpY  ^  ^ 

-[{xp  -  xY  +  {z  -  ZpY]69  +  (z  -  Zp)6xp  +  {x 


Zp)6x  +  {xp 

Xp)6zp  ]■ 


x)6z 


Furthermore,  without  loss  of  generality,  in  the  last  equation  we  set  Zp  =  0: 
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Xf  Xp  -Xc+{z-Zp  Wc  _  I  ^  ^ 


—  {xp  —  xY  +  z‘^]S9  +  zSxp  +  (a;  —  Xp)5zp  } 


During  the  initial  geo-location  phase  where  one  is  exclusively  interested  in  the  ground 
object’s  position,  one  takes  the  INS  calculated  aircraft  navigation  state  variables  x,  z,  9 
at  face  value  and  the  linearized  measurement  equation  is  used  to  estimate  5xp,  Szp  : 


Xf  Xp  -  X  +  {z  -  Zp  )9 
f  z-  Zp  +{x-  Xp)9 


z)6xp  +  {x  —  Xp  )Szp 


(3.20) 


This  equation  would  be  used  if  a  recursive  geo  -  location  algorithm  is  applied.  Note 
however  that  if  one  is  exclusively  interested  in  geo  -  locating  the  ground  object,  a 
batch  algorithm  might  be  preferable.  Finally,  one  would  use  Equation  (3.19)  and 
iterate 


=  Xp  —  Sxp 

(3.21) 

-  Zp  -  Szp 

(3.22) 

In  practice,  the  hrst  guesses  of  Xp  and  Zp  are  generated  as  follows.  We  have  the  INS 
provided  “prior”  information  on  the  navigation  state  x,  z,  9.  Bearing  measurements  of 
the  ground  feature  P  are  taken  over  time.  One  can  use  the  hrst  two  bearing  measure¬ 
ments  to  obtain  the  “prior”  Xp  and  Zp  information.  Hence,  initially  one  is  exclusively 
concerned  with  the  geo  -  location  of  the  feature  on  the  ground.  To  this  end  one  uses 
Equation  (3.3)  and  upon  recording  two  measurements  one  obtains  a  set  of  two  linear 
equations  in  the  unknowns  Xp  and  Zp  : 

Xp  -|-  COt(0ci  T  ^mYZp  X^i  T  COt(0cj^  -|-  i^rni)  (3.23) 
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Xp  -\-  cot  (0^2  “1“  Xc2  ^C2  cot  (0^2  “1“  'Cm2) 


(3.24) 


whereupon 


(  ^ 

\  1 

cot(9c2  4“  Cm2) 

cot(0ci 

\  ) 

1  COt(0(;;2“4Cm2)~COt(0cj^4-Cmj^) 

-1 

1 

(  Xqi  “t"  ^ci  COt(0cj  -|-  ^mi  ) 

yXc2  4*  ^C2  COt(0C2  4"  Cm2) 


Obviously,  one  could  use  more  than  two  bearing  measurements  and  obtain  an 
overdetermined  linear  system  in  Xp  and  Zp,  but  then  one  performs  geo- location  only 
and  completely  forgoes  the  task  of  INS  aiding.  Since  at  time  instants  k  =  1  and 
k  =  2  the  INS  is  not  aided,  the  original  prior  information  on  the  navigation  state  eror 
Sx~,  Sz~ ,  69~  must  be  propagated  forward  to  time  step  k  =  2.  From  this  point  on, 
the  updated  prior  information  dx~,  Sz~,  69~  and  the  prior  information  Xp  and  Zp 
are  employed  to  start  the  Kalman  hlter  such  that  the  aircraft  navigation  state  and 
the  ground  object’s  position  are  simultaneously  updated  using  the  bearing  measure¬ 
ments  obtained  at  time  k  =  3,4....  In  conclusion:  When  unknown  ground  objects  are 
tracked,  they  hrst  must  be  geo  -  located.  This  delays  the  INS  aiding  action  by  at 
least  two  time  steps.  Furthermore,  the  aircraft’s  navigation  state  prior  information 
must  be  propagated  ahead  two  time  steps,  without  it  being  updated  with  bearing 
measurements,  while  one  exclusively  relies  on  the  INS.  The  measurement  equation  is 
then  re-  linearized  using  the  ground  object’s  prior  information  obtained  during  the 
preliminary  geo  -  location  step,  and  the  two  time  steps  propagated  ahead  navigation 
state  prior  information.  From  this  point  on,  the  INS  aiding  action  and  the  ground 
object’s  geo  -  location  is  simultaneously  performed  during  the  ground  object’s  track¬ 
ing  interval. 
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If  the  ground  object’s  elevation  Zp  is  known,  say  from  a  digital  terrain  data-base, 
one  can  make  do  with  one  bearing  measurement  only: 

Xp  Xc  (^c  ^p)  COt(^c  ^m)  (3.25) 


3.2.4  Nondimentional  Variables.  The  aircraft’s  nominal  altitude  is  h  and 
its  nominal  ground  speed  is  v.  Set 


X 


-  z 

/l’  ^ 


6uj 
a  ’ 


Introduce  the  non-dimentional  parameter  the  ratio  of  the  aircraft’s  potential  energy 
to  its  kinetic  energy): 


Then  the  aircraft’s  non-dimentional  navigation  state  error  dynamics  are  specihed  by 
the  matrices 
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0 

0 

1 

0 

0 

0 

0 

0 

0 

0 

0 

1 

0 

0 

0 

0 

0 

0 

0 

0 

a 

,r  = 

a 

0 

0 

0 

0 

0 

0 

0 

0 

a 

0 

0 

0 

0 

0 

0 

0 

0 

1 

(3.26) 


The  scenario  considered  is  wings  level  flight  in  the  vertical  plane  {x,z). 

For  example,  for  a  MAV,  v  =  20  [^],  h  =  40  [m],  and  g  =  10  [^]  a  =  1. 


The  geometry  of  the  measurement  scenario  is  characterized  by  the  two  non-dimensional 
parameters 

tti  =  tan?7i 

a2  =  tan?72 

Consequently,  the  non-dimensional  measurement  interval 

T  =  tan ?7i -|- tan ?72  (3.27) 

Consider  hrst  a  symmetric  measurement  scenario  where  rji  =  ri2  =  V-  Since  x  = 
vt,  Xp  =  /itan?7,  using  nondimensional  variables,  we  obtain  from  the  measurement 
Equation  (3.17) 


y  =  5x  +  (tan?7  —  t)Sz  —  [1  +  (tan  77  —  tY]S9,  (3.28) 

0  <  t  <  2tan?7 
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z 


V 


h 


Figure  3.2:  Simple  Measurement  Scenario 
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i.e.,  the  measurement  matrix 


C 


1,  tan  77  — t,  0,  0,  2t  tan  ?7  — 


(3.29) 


the  measurement 


Zc  (^Xc  XpjOc 


and  the  measurement  interval 


(3.30) 


T  =  ^(tan?7i  +  tan  772) 

Note  that,  as  is  often  the  case  in  INS  aiding,  the  measurement  matrix  C  is  time  de¬ 
pendent. 


3.2.5  Observability.  The  observability  Grammian  [12]  is 


W{T)  = 


(3.31) 


where  T  is  the  measurement  interval. 


We  calculate 
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1  0  t  0 

0  1  0  t  0 

e^*=  0  0  1  0  at  (3.32) 

0  0  0  1  0 

0  0  0  0  1 

The  geometry  of  the  symmetric  measurement  arrangement  is  specihed  by  the  non- 
dimensional  parameter  a  =  tan?].  Then  the  non-dimentional  observation  interval 
T  =  2a  and  the  measurement  matrix 

C{t)  =  1,  a  —  t,  0,  0,  — 1  —  (a  —  (3.33) 

We  calculate 

C{t)e^^  =  1,  a  —  t,  t,  {a  —  t)t,  —  1  —  (a  —  t)^  (3.34) 

and 

1  a  —  t  t  {a  —  t)t  fit) 

a  —  t  (a  —  t)^  {a  —  t)t  {a  —  t)H  {a  —  t)  fit) 

e^"^C^{t)C{t)e^^=  t  {a-t)t  t^  {a  -  t)t^  tf{t) 

{a  —  t)t  {a  —  t)‘^t  {a  —  t)f^  {a  —  t)H‘^  {a  —  t)tf{t) 

fit)  ia-t)f{t)  tf{t)  t{a-t)f{t)  p{t) 

(3.35) 

where  f{t)  =  \af^  —  1  —  (a  —  t)^.  We  integrate  the  time-dependent  entries  of  the 
matrix  in  Equation  (3.35)  and  obtain  the  observability  Grammian  elements. 


hTi,i  —  2q;,  hTi^2  —  0,  Wi^3  —  2a‘^ ,  Wi^4  —  — — —  —aa^  —  2a  —  — (3.36) 

o  o  o 
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(3.37) 


^2,2  =  W2,3  =  ^2,4  =  W2,5  =  2a^  -  2a^  -  ‘^aa'^, 

W^3,3  =  W"3,4  =  W^3,5  =  2aa^  -  -  2^2,  (3.38) 

16  2  6  2 

1^4,4  =  —a^,  7^4,5  =  -a^  -  -aa^  +  -a^,  (3.39) 

15  5  5  3 

5  =  ("IT  —  TT®  “1“  —0^)0;^  +  — (1  —  2a)oi^  +  2a  (3.40) 

5  15  5  3 

Consider  the  MAV  scenario  where  a  =  1  and  the  measurement  scenario  shown 
in  the  Figure  3.2,  where  a  =  1 


The  observability  Grammian  is  then 


15 

0 

15 

-5 

-10 

0 

5 

-5 

5 

-5 

15 

-5 

20 

-10 

-5 

-5 

5 

-10 

8 

-1 

-10 

-5 

-5 

-1 

12 

(3.41) 


The  5x5  real,  symmetric,  positive,  semi-dehnite  matrix  W  is  not  full  rank:  Rank 
{W)  =  3.  This  implies:no  observability. 


Next,  consider  the  alternative  measurement  geometry  where  the  tracked  ground  fea- 


ture  P  is  at  the  origin  {xp  = 

=  0).  Then  77  =  0 

so  that  a 

=  0  and 

1 

—t 

t 

-e 

fit) 

—t 

-e 

-fit)  ■  t 

t 

-e 

-t^ 

fit)  ■  t 

-e 

-t^ 

-fit)  • 

fit) 

-fit)  ■  t 

fit)  ■  t 

-fit)  •  t^ 

2fit) 
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where  f{t)  =  |(a  —  l)t^  —  1 


The  non-dimensional  measurement  interval  T  =  2,  and  the  elements  of  the  observ¬ 
ability  Grammian  are 


=  2,  W,,2  =  -2,  W"i,3  =  2,  fTi,4  =  -|,  W^i,5  =  i(2a  -  7), 
m,2  =  f,  W2,3  =  -|,  W2,4  =  4,  1^2,5  =  2(3  -  a), 
fh-3,3  =  |,  1^3,4  =  -4,  1^3,5  =  2(a  -  3), 

fT4,4  =  f,  W^4,5  =  ^(17-6a), 

144,5  =  ~  1)^  +  §(2  —  a)  +  2 

As  before,  assume  a  =  1.  The  observability  Grammian  is 


15 

-15 

15 

-20 

-25 

-15 

20 

-20 

30 

30 

15 

-20 

20 

-30 

-30 

-20 

30 

-30 

48 

44 

-25 

30 

-30 

44 

47 

(3.42) 


The  matrix  W  has  two  identical  columns  (columns  2  and  3)  and  two  identical  rows, 
rows  2  and  3.  This  implies  Rank  (W)  =  3. 


When  both  features  are  tracked,  the  observation  matrix  is  the  2x5  matrix. 
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1  1-t  0  0  2t-t^-2 


(3.43) 


-t  0  0 


As  before,  for  a  =  1  we  obtain 


e 


At 


1  0  t  0 

0  1  0  t  0 
0  0  1  0  t 
0  0  0  1  0 
0  0  0  0  1 


and  we  calculate 


1  1-t  t  2t-\t^  -2 

1  -t  t 


and  = 


2 

l-2t 

2t 

t-2t^ 

1  -  2t 

l  +  2t^  -  2t 

t  -  2t^ 

t-2e  +  2t^ 

2t 

t-2t^ 

2e 

-  2t^ 

t  -  2t^ 

t-2e  +  2t^ 

-  2t^ 

2t^  -  2t^  + 

2t-t^  -3 

+  5t  -  2 

2t^  -t^-3t 

+  5t^  - 

Integration  yields  the  entries  of  the  observability  Grammian 


(3.44) 


(3.45) 


2t-t^-3 
-\t^  +  ht-2 
2t^ 

+  5t2  -  2t 

^-2t^  +  7t‘^  -  8t  +  5 
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14^1^1  —  30,  14^1,2  —  — 15,  44^1^3  —  30)  44^i,4  —  — 25,  44^1^5  —  — 35, 

44^2,2  =  25,  44^2,3  =  “25,  44^2,4  =  35,  44^2,5  =  25, 

44^3,3  =  40,  44^3^4  =  —40,  44^3^5  =  —35, 

444i^4  =  56,  44^4^5  =  43, 

44^5,5  =  59 

The  matrix  W  is  full  rank.  Hence,  we  have  observability. 


Concerning  a  shortcut:  A  sufficient  condition  for  observability  is  rankiW)  =  5,  where 
44^  =  44^p^  +  44^p2  and  44^^^  is  the  observability  Grammian  when  ground  object  i  is 
tracked,  f  =  1,  2;  this  is  indeed  the  case. 

3.2.6  Only  the  Elevation  Zp  of  the  Tracked  Ground  Object  is  Known.  The 
augmented  state’s  error  is  {6x,  6z,  Sv^,  Sv^,  S9,  6xp)'^  G  3?®.  Set  Xp 


The  augmented  system’s  dynamics  are  specihed  by 


Aa  := 

A  : 

:  0 

,r,  := 

T 

0  ! 

:  0 

6x6 

0 

a(t):=(C(t),-l)  (3.47) 
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where,  recall,  for  wings  level,  constant  altitude  flight. 


and 


0 

0 

1 

0 

0 

0 

0 

0 

0 

0 

0 

1 

0 

0 

0 

0 

0 

0 

0 

0 

a 

,r  = 

a 

0 

0 

0 

0 

0 

0 

0 

0 

a 

0 

0 

0 

0 

0 

0 

0 

0 

1 

C{t)  = 


1,  a  —  t,  0,  0,  2at  — 


a 


(3.48) 


(3.49) 


We  calculate 


:= 


^At 


0 


(3.50) 


a 


^Aat 


=  (Ce*  -1) 


(3.51) 


and 


e^"*C'J(t)a(t)e^“*  = 


Aat 


-Ce 


At 


(3.52) 
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(3.53) 


(1,  a  —  t,t,at  —  —  1  — 


+  2at) 


Let 


r-2a 


T  _ 

w  = 


2,3  2 


C{t)e^Ut  =  (2a,  0,  2a^, -^a^,  ^(2a  -  l)a^  -  2a) 

o  o 


Wa  = 


When  a  =  1, 


(3.54) 

(3.55) 


2  3  2  3 


w  =  (2a,  0,  2a  ,  —-a'^,  -ck'*  —  2a) 


(3.56) 


and  for  a  =  1, 


w 


T 


(2.0.2.-|-i) 


(3.57) 


When  a  =  0, 


w 


T 


(2, 


-2,2, — , 

,  ,  3, 


(3.58) 
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Hence,  when  a  =  1  and  a  =  1, 


When  a 


Wi  = 


2 

15 


1  and  a  =  0 


W2  = 


2 

15 


15 

0 

15 

0 

5 

-5 

15 

-5 

20 

-5 

5 

-10 

-10 

-5 

-5 

-15 
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-15 
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-25 
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15 

-15 

-5 

-10 
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-5 

0 

-10 

-5 

-15 

8 

-1 

5 

-1 

12 

10 

5 

10 

30 

-20 

-25 

-15 

30 

30 

15 

-30 

-30 

-15 

48 

44 

20 

44 

47 

25 

20 

25 

15 

Rank(Wi)  =  4.  and  Rank  (W2)  =  3.  Rank  (Wi  +  W2)  =  6. 


(3.59) 


(3.60) 


3.2.7  Partial  Observability.  When  the  observability  Grammian  W  is  rank 
dehcient,  that  is, 


rankiW)  =  r  <  n  , 

where  n  is  the  state  space  dimension,  the  system  is  not  observable;  we  then  refer 
to  the  system  as  being  partially  observable.  Strictly  speaking,  the  aiding  action  of 
bearing-only  measurements  does  not  percolate  into  all  the  state  components.  One  is 
thus  interested  in  determining  which  states  are  (positively)  impacted  by  the  aiding 
action.  The  answer  is  provided  by  the  Singular  Value  Decomposition  (SVD)  of  the 
observability  Garammian  matrix  W.  The  following  holds: 
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The  n  X  n  real  symmetric  positive  semi-definite  matrix  W  can  be  factored  as 


W  =  HK 

where  H  is  a  n  x  r  matrix  and  K  is  a  r  x  n  matrix.  Furthermore,  rank{H)  =  r  and 
rank{K)  =  r;  in  other  words,  this  is  a  full  rank  factorization. 

From  the  SVD  we  conclude  that  the  measurement  process  allows  us  to  estimate  the 
parameter  6  &  W: 

Jo 

The  latter  is  related  to  the  navigation  state  X  as  follows. 

6  =  KXo  (3.61) 

The  navigation  information  provided  by  the  bearing  measurements  is  encapsulated  in 
Equation  (3.61).  The  complete  initial  state  Xq  cannot  be  calculated  from  the  mea¬ 
surement  record  y{t),  0  <t  <T  and  in  order  to  obtain  a  data  driven  estimate  of  the 
full  state  vector,  n  —  r  additional  independent  measurements  of  the  navigation  state 
are  needed.  In  2-D,  and  when  the  position  of  the  ground  object  is  known,  the  dimen¬ 
sion  of  the  state,  n  =  5.  When  one  known  ground  feature  is  tracked,  r  =  3.  This  tells 
us  that  two  additional  independent  measurements  of  the  navigation  state  are  needed. 
The  availability  of  the  passively  measured  baro  altitude  immediately  comes  to  mind, 
so  that  one  additional  independent  measurement  is  needed  for  establishing  the  navi¬ 
gation  state.  The  latter  could  be  the  aircraft’s  pitch  angle  6,  which  is  independently 
provided  by  a  vertical  gyroscope. 
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3.3  Flying  in  3-D 

Scenario:  Nominal  aircraft  trajectory  is  wings  level,  constant  altitude  flight. 

Zn 


Figure  3.3:  The  Navigation  and  Body  frames  in  3  -  D. 

The  information  on  the  attitude  errors  is  encapsulated  in  the  vector  (5T.  The 
latter  is  not  listing  of  the  aircraft  Euler  angles  errors.  We  first  derive  an  expression 
for  5T  as  a  function  of  the  Euler  angles’  errors. 

In  this  section  the  relationship  between  the  “attitude  error”  vector  (jT  and  the 
Euler  angles  errors  69,  S'lp,  64>  is  established. 

First,  note  that  the  nominal  navigation  to  body  axes  DCM  is 


0  1  0 

1  0  0 

0  0-1 


(3.62) 
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Next,  consider  three  relationships: 


Zn 


Fignre  3.4:  6  -  Rotation. 

3.3.1  6  -  rotation.  Since 

Un  =  Xb  cos  9  +  Zb  sin  9 
Zn  =  Xb  sin  9  —  Zb  cos  9 

Xn  Vb 


r  1 

0  1  0 

Xb 

Un 

= 

cos  6*  0  sin  6* 

Vb 

\  / 

sin  9  0—  cos  9 

V  / 
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0 


0 


2 


cm 


and  for  small  angles  66, 


1 

cos  6*  0 
sin  6*  0 


sin  6* 

-  cos  9 


cr 


0  1  0 

1  0  66 

66  0  -1 


Fignre  3.5:  V’  ■  rotation. 
3.3.2  tfj  -  rotation.  Since 

Un  =  Xb  cosV'  -  UbSintl: 

Xn  =  Xb  sin  '0  +  2/6  cos  ip 
Zn  =  -Zb 
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f  X  \ 

sin  0  cos  0  0 

Un 

= 

cos  0  —  sin  0  0 

Vb 

\  y) 

0  0-1 

V  / 

and  for  small  angles 


sin'^ 

COS'^ 

0 


COS'^ 

—  sin'^ 
0 


0 

0 

-1 


c, 


n 

b 


Si/j  1  0 

1  —5ip  0 
0  0-1 


3.3.3  0  -  rotation.  Since 


Xn  =  Xb  cos  (j)  —  Zb  sin  0 
Zn  =  —Zb  COS  <p  —  Vb  sin  0 


Un 


f  X  \ 

0  cos  0  —  sin  0 

Vn 

= 

1  0  0 

Vb 

) 

0  —  sin  0  —  cos  0 

\  / 
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Zn 


Figure  3.6:  V’  ■  rotation. 


cm 


and  for  small  angles  50, 


0  cos  0  —  sin  0 

1  0  0 
0  —  sin  0  —  cos  0 


cm 


0  1  —50 

1  0  0 
0  -50  -1 


where  from  we  finally  conclude 


Since 


^7,^50, 50, 50)  = 


50  1  —50 

1  —50  50 

0  —50  —1 
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0  1  0 

^,-(0,0,0)=  1  0  0 

0  0-1 


Now, 


5il)  0  —50 

=  0  -50  59 

0  —50  0 


5\1/ 


-5C^  ■ 

50  0  —50 

0  —50  59 

0  —50  0 


0  1  0 

1  0  0 

0  0-1 


0  —50  —50 

5^  =  50  0  59 

5(j)  —59  0 

where  from  we  calculate  the  “attitude  error”  vector 
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Hence,  the  navigation  state’s  error  dynamics  are 


5 

Tt 


0  0  0  1  0  0  0 

0  0  0  0  1  0  0 

0  0  0  0  0  1  0 

0  0  0  0  0  0  0 

0  0  0  0  0  0  g 

0  0  0  0  0  0  -a 

0  0  0  0  0  0  0 

0  0  0  0  0  0  0 

0  0  0  0  0  0  0 


0 

0 

0 

-9 

0 

0 

0 

0 

0 


(3.63) 


3.4  Measurement  Equation 

From  the  geometry  of  the  measurement  arrangement,  the  “Main  Equation”  is 
obtained: 
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(3.66) 


X 


+  (Zj,  —  z] 


Xfi)  -f(j)  +  yf 
Xf6  —  yf(j)  —  f  I  j-Q  _  y^(j) 


In  the  special  case  of  2-D  flight,  we  have  =  0 


yp-y  = 


Zp-  z 
XfO-f 


ifO  +  Xf) 


i.e., 


yp-y  ^  XfO  +  fe 

Z-Zp  f-Xf9 


(3.67) 


(3.68) 


Setting  —  =  tan  (  yields 


yp  —  y  1  +  tanC,  ■  9 

z  —  Zp  tan  C  —  6* 

l  +  tanCtaD.9  ,rn  ,, 

= - - - —  and  jor  9  small 

tanC  —  tant^ 

1 

tan(C  —  9) 


Vp-y 

Z-Zp 


cot(C  -  9), 


as  before;  note  the  polarity  of  9. 


In  the  general  3-D  case  we  have 
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Xp  —  X  =  {z 


Xfjj  -  f(j)  +  yf 
f  +  yf<p-  XfO 


yp-y  =  {z 


fe  -  yfjj  +  Xf 
f  +  yf<P- XfO 


(3.69) 

(3.70) 


Xp  —  X  =  {z 


-  0  +  J 


yp-y  =  {z 


.o-'fi'  +  J 
+  ^0-  ^6 


(3.71) 

(3.72) 


Set 


X 


p 


Xp  —  X  =  {z 


Xfjj  -  (l)  +  yf 
1  +  I//0  -  XfO 


yp-y  =  {z 


9  -yfjj  +  Xf 
1  +  I//0  -  XfO 


The  small  angles  approximation  yields 


(3.73) 

(3.74) 
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Xp-X=  {z-  Zp){xf'ip  -  0  +  |//)(1  -  Vfcj)  +  Xf9) 
=  {z-  Zp){xfij  -  (j)  +  yf  -yj(j)  +  XfyjO) 


and 


yp-y  =  {z-  Zp){9  -  yftjj  +  Xf){l  -  yf(^  +  Xf9) 
=  {z-  Zp){9  -  yftjj  +  Xf  +  x)9  -  Xfyf<t)) 


Xp-x  =  {z-  Zp)  [yf  +  Xf'ip  +  Xfyf9  -  (1  +  |/J)0] 
yp-y  =  {z-  Zp)  [xf  -  yf'tp  +  (1  +  x])9  -  Xfyf(p] 


Thus 

Xp  -  X  +  {zp  -  z)yf  +  ZpXf'ip  +  zpXfyf9  -  Zp{l  +  yj)(j)  =  z[xfij  +  Xfyf9  -  (1  +  yj)(i>] 


yp-y  +  {zp- z)xf  +  Zpyfi)  +  Zp{l+xj9)  - ZpXfyf(j)  =  z[-yf%l)  +  {l  +  xj)9- xjyfcj)] 


3.4-1  Small  Perturbations. 

X  =  Xc-  Sx,  y  =  yc-  Sy,  z  =  Zc-  Sz,  9  =  9c-  69,  =  ^Jc-  6iIj,  (p  =  4>c-  64> 
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Assume  the  position  Xp,  i/p,  Zp  of  the  ground  feature  is  known. 


Xp-  Xc  +  Sx  +  {zp  -  Zc)yf  +  Szyf  +  ZpX  ft/jc  -  ZpX  fy  f69  -  Zp{l  +  y‘j)4>c  +  Zp{l  +yj)64> 

=  ZcixfiJc  -  Xf5i)  +  xjyjOc  -  Xfyf69  -  (1  +  y})(j)c  +  (1  +  yj)^]  -  Sz[xf^lJc  +  Xfyf9c  - 
(1  +  yj)(j)c\ 


yp  -  yc  +  Sy  +  {zp  -  Zc)xf  +  6zxf  -  Zpyf^Jc  +  Zpyf5%l)  +  ^p(l  +  x‘j)9c  -  ZpXjyjcjyc  + 

ZpXfyjScf)  —  Zp{l  +  x‘^f)59 

=  Zci-yf'ipc  +  yfS^|J  +  (1  +  xj)9c  -  (1  +  xj)S9  -  Xfyf(t)c  +  a;/|//(50]  -  Sz[yfiJc  +  (1  + 
x‘j)9c  -  XfyfCpc] 


Xp-Xc+  {zp  -  Zc)yf  +  ZpXf^Jc  +  ZpXfyf9c  -  Zp{l  +  yj)(pc  -  ZcXf^j^  -  ZcXfyf9c  + 

Zc{l  +  y})(l)c 


=  -Sx  -  [xf-ipc  +  Xfyf9c  -  (1  +  y})(j)c  +  yf]Sz  +  [zpXfyf  -  ZcXfyf]S9  +  {zc  -  Zp){l  + 

yj)S(l)  +  {zpXf  -  ZcXf)6ij 
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yp-yc  +  {Zp  -  Zc)xf  -  ZpVfiJc  +  Zp{l  +  x))0c  -  ZpXfyf(j)c  +  Zcy/i^c  -  Zc{l  +  xj)ec  + 

ZcX  fy  f(j)c 

=  -5y  -  [(1  +  xj)9c  -  yf^c  -Xfyjcpc  -Xf]5z  +  (1  +  xj){zp  -  z^bd  +  Xfyf{zc  -  Zp)5(l)  + 

yf{zc-  Zp)6^/j 


Xp-Xc+{zc-Zp)[{l  +  yj)4>c-Xfyfdc-XfilJc-yf]  =  -bx-[yf  +  Xfi)c  +  Xfyf9c- 
(1  +  y‘j)(t)c]bz  +  {zp  -  Zc)[xfyfb9  +  Xfbip  -  (1  +  yfjbcf)] 


yp-yc+  {Zc  -  Zp)[yf%l)c  -  Xfyf^c  -  (1  +  x))9c  -  Xf]  =  -by  +  [xf  +  yf^Jc  +  Xfyf(j)c  - 
(1  +  xj)9c]bz  +  {Zp  -  2:c)[(l  +  xj)b9  -  Xjyfbcf)  -  yfb^)] 


On  the  RHS  of  the  above  two  equations,  set  'ipc  =  9c  =  (pc  =  ^  (the  nominal  trajectory 
is  wings  level  flight). 


Xp-Xc  +  {zc- Zp)[{l+y‘j)(i)c-Xfyf9c-Xf%l)c-yf]  =  -bx -yfbz  +  {zp- Zc)[xfyfb9 - 
(1  +  yj)b(l)  +  Xfbpj] 


yp-yc+  {Zc  -  Zp)[xf  +  (1  +  x‘j)9c  -  XfyjOc  -  yfipc]  =  -by  +  Xfbz  +  {zp  -  Zc)[{l  + 
xj)b9  -  xjyjbcj)  -  yjb'ip] 
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Also,  on  the  RHS  on  the  above  equations,  set  2:c  =  h. 

Xp- Xc+{zc- Zp)[{l+yj)(t)c- X  fV  fdc-x  fi)c-y  f\  =  -5x-yf5z+{zp-h)[xfyf5d- 

{l  +  y‘j)5(l)  + Xf5%lj] 

yp-yc  +  {zc-Zp)[xf  +  {l  +  x))9c-Xfyf9c-yf'4)c]  =  -5y  +  Xf5z  +  {zp-h)[{l  +  x))59- 

XfVfScj)  -  yfS'if)] 

Finally,  without  loss  of  generality,  assume  Zp  =  Q 

Xp-  Xc  +  Zc[{l  +  y})4'c  -  Xfyf9c  -  Xf'ipc  -  yf]  =  Sx  -  yjSz  -  h[xfyfS9  -  (1  + 
yj)S(f)  +  XfSij] 

yp-yc+Zc[xf  +  {l+xj)9c-Xfyf9c-yfi)c]  =  -Sy+XfSz-h[{l+x‘j)69-Xfyf6(l)-yfS'ip] 

Non-dimensionalize  by  dividing  by  h: 


Xp  Xc  Zc  ^  6x  ^  6y  ^  6z 

Xp  ^  y Ac  ^  ox^  —,6y  ^  ~h 
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Xp-  Xc  +  Zc[yf  +  XfUfOc  +  Xf^Jc  -  (1  +  |//)0c]  =  -5x  -  yf5z  -  Xfyf69 
+(1  +  yj)S(l)  -  Xf6ip 

yp-yc  +  Zc[xf  +  (1  +  xj)9c  -  Xfyf9c  -  yjiJc]  =  Sy  +  XfSz  -  (1  +  x‘j)S9 

+Xfyf6(j)  +  yf6^/j 


In  3-D  and  wings  level  flight  the  measurements  matrix 


C{t) 


-1  0  -y/  0  0  0  -Xfyf  ^  +  y}  -Xf 

0  —1  Xf  0  0  0  —{1  +  x‘f)  Xfyf  yf 


Note: 


In  the  special  2-D  Case 


Xf  =  Xfit) 


Cit) 


— 1  0  —Xf  0  0  1  + 


(3.75) 


as  expected. 

The  solution  method  for  hnding  the  point  of  “Intersection”  of  Two  Directed 
Straight  Lines  in  is  explained  in  detail  in  Appendix  A. 
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IV.  Results  and  Analysis 


4.1  Development  of  Kalman  Filtering  for  Flight  Scenarios 

State  representation  for  flight  scenarios  was  obtained  in  Chapter  3.  Here  we 
need  to  state  discrete  Kalman  Filtering  equations. 

Sate: 

x{k)  =  ^{k,k  —  l)x{k  —  1)  +  B{k  —  l)u{k  —  1)  +  G{w{k  —  1))  (4.1) 

Measurement: 

Z{k)=  H{k)x{k)  +  V{k)  (4.2) 

Propagation: 


x-{k)  =  ^{k,k-l)x-^{k-l)  (4.3) 

p-{k)  =  4>(A;,  k  -  1)P+(A;  -  1)$(A;,  k  -  if  +  GQG^  (4.4) 

Update: 

K{k)  =  p-{k)H^{k)[H{k)p-{k)H^{k)  +  R{k)]-^  (4.5) 

x+{k)  =  x-{k)  +  K{k)[Z{k)  -  H{k)x-{k)]  (4.6) 

P+{k)  =  p-{k)  -  K{k)H{k)p-{k)  (4.7) 
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4-2  Scenario  1 

Here  in  this  case  we  have  almost  perfect  level  flight  in  3  -  D.  Euler  angles 
are  assumed  negligible.  Accelerometers  of  the  INS  have  bias  O.Olm/s^,  initial  state 
estimate  and  covariance  are  set  as  random,  and  measurement  and  also  position  of 
the  UAV  calculated  based  on  measurement  assumed  perfect.  Here  we  have  1  m/s^ 
constant  acceleration  in  y  direction  (North).  Flight  level  is  500  meters.  16  objects 
are  generated  randomly.  Ten  measurements  are  obtained.  In  this  scenario  we  tried 
algorithm  for  one  object  per  measurement.  Positions  of  ground  objects  are  assumed 
known.  Error  plots  from  Kalman  filter  are  shown  below,  and  all  other  plots  are  placed 
in  Appendix  B. 
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Kalman  Filter  Position  Error  (Est  -  True) 
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Figure  4.1;  Scenario  1.  Kalman  Filter  Position  Errors.  Estimated  -  true.  Red  is 
+  —  standard  deviation. 
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400 


Kalman  Filter  Velocity  Error  (Est  -  True) 


Time  (sec) 


Figure  4.2:  Scenario  1.  Kalman  Filter  Velocity  Errors.  Estimated  -  true.  Red  is 
+  —  standard  deviation. 


Table  4.1:  Standard  deviation  results  obtained  from 
Kalman  filtering  of  scenario  1. 


Standard  Deviation 

Value 

28.3415  (m) 

ay 

26.7456  (m) 

C^Z 

24.8587  (m) 

^VX 

5.8403  (m/s) 

^vy 

5.5674  (m/s) 

O'vz 

1.1592  (m/s) 

0-0 

0.1031  (rad) 

^  (j) 

0.1916  (rad) 

0.9892  (rad) 
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4-3  Scenario  2 

Here  in  this  scenario  we  have  almost  same  conditions.  Euler  angles  are  assumed 
negligible.  Accelerometers  of  the  INS  have  bias  O.Olm/s^,  initial  state  estimate  and  co- 
variance  are  set  as  random,  and  measurement  and  also  position  of  the  UAV  calculated 
based  on  measurement  assumed  perfect.  Here  we  have  1  m/s^  constant  acceleration 
in  y  direction  (North).  Flight  level  is  500  meters.  16  objects  are  generated  randomly. 
Ten  measurements  are  obtained.  In  this  scenario  we  tried  algorithm  for  two  object 
per  measurement.  Positions  of  ground  objects  are  assumed  known.  Error  plots  from 
Kalman  hlter  are  shown  below,  and  all  other  plots  are  placed  in  Appendix  C. 


Kalman  Filter  Position  Error  (Est  -  True) 


Time  (sec) 

Figure  4.3:  Scenario  2.  Kalman  Filter  Position  Errors.  Estimated  -  true.  Red  is 
standard  deviation. 
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Kalman  Filter  Velocity  Error  (Est  -  True) 
1000 1 - ^ ^ ^ - 


-500' - ^ ^ ^ ^ - 

0  20  40  60  80  100 


> 


-500' - ^ ^ ^ ^ - 

0  20  40  60  80  100 


Figure  4.4:  Scenario  2.  Kalman  Filter  Velocity  Errors.  Estimated  -  true.  Red  is 
+  —  standard  deviation. 


Table  4.2:  Standard  deviation  results  obtained  from 
Kalman  filtering  of  scenario  2. 


Standard  Deviation 

Value 

26.0850  (m) 

ay 

17.5229  (m) 

C^Z 

23.7029  (m) 

^VX 

5.6905  (m/s) 

^vy 

4.9955  (m/s) 

O'vz 

1.1419  (m/s) 

0-0 

0.1005  (rad) 

^  (j) 

0.1845  (rad) 

0.9216  (rad) 
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4-4  Scenario  3 


Same  conditions  applied  in  this  scenario  for  three  ground  objects.  Euler  angles 
are  assumed  negligible.  Accelerometers  of  the  INS  have  bias  O.Olm/s^,  initial  state 
estimate  and  covariance  are  set  as  random,  and  measurement  and  also  position  of 
the  UAV  calculated  based  on  measurement  assumed  perfect.  Here  we  have  1  m/s^ 
constant  acceleration  in  y  direction  (North).  Flight  level  is  500  meters.  16  objects 
are  generated  randomly.  Ten  measurements  are  obtained.  In  this  scenario  we  tried 
algorithm  for  three  object  per  measurement.  Positions  of  ground  objects  are  assumed 
known.  Error  plots  from  Kalman  hlter  are  shown  below,  and  all  other  plots  are  placed 
in  Appendix  D. 


Kalman  Filter  Position  Error  (Est  -  True) 
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Figure  4.5:  Scenario  3.  Kalman  Filter  Position  Errors.  Estimated  -  true.  Red  is 
+  —  standard  deviation. 
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Figure  4.6:  Scenario  3.  Kalman  Filter  Velocity  Errors.  Estimated  -  true.  Red  is 
+  —  standard  deviation. 


Table  4.3:  Standard  deviation  results  obtained  from 
Kalman  filtering  of  scenario  3. 


Standard  Deviation 

Value 

21.7347  (m) 

ay 

17.2366  (m) 

C^Z 

18.4061  (m) 

^VX 

5.3178  (m/s) 

^vy 

4.9784  (m/s) 

O'vz 

1.0640  (m/s) 

0-0 

0.1004  (rad) 

^  (j) 

0.1841  (rad) 

0.9264  (rad) 
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4-5  Scenario  4 


Here  in  this  scenario  we  repeated  conditions  for  two  objects  like  in  scenario 
2.  Positions  of  object  reflection  on  the  camera  scene  were  obtained  as  in  previous 
scenarios.  Then  this  information  was  used  as  input  for  the  methodology  explained 
in  Appendix  A.  Measured  object  positions  are  very  different  at  third  dimension  z. 
This  could  be  because  of  non-dimentialisation  used  in  our  methodology.  We  used 
input  from  barometric  altimeter  to  correct  this  error.  Firstly  we  ran  Kalman  hlter 
for  perfect  measurement.  Then  we  replaced  perfect  measurements  with  measurement 
of  our  algorithm,  and  we  ran  Kalman  hlter  again  to  compare  results.  Plots  for  both 
runs  are  shown  below. 


Kalman  Filter  Position  Error  (Est  -  True) 


Figure  4.7:  Scenario  4.  Kalman  Filter  Results  from  Perfect  Measurement.  Kalman 
Filter  Position  Errors.  Estimated  -  true.  Red  is  H —  standard  deviation. 
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Kalman  Filter  Velocity  Error  (Est  -  True) 


Time  (sec) 


Figure  4.8:  Scenario  4.  Kalman  Filter  Results  from  Perfect  Measurement.  Kalman 
Filter  Velocity  Errors.  Estimated  -  true.  Red  is  H —  standard  deviation. 


Table  4.4:  Standard  deviation  results  obtained  from 
Kalman  filtering  of  scenario  4. 


Standard  Deviation 

Value 

25.3797  (m) 

ay 

17.5103  (m) 

(Tz 

24.0239  (m) 

^VX 

5.6623  {m/s) 

^vy 

4.9949  (m/s) 

1.1304  (m/s) 

(ye 

0.1005  (rad) 

^(p 

0.1892  (rad) 

0.9849  (rad) 
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Table  4.5:  Calculated  object  positions  for  Scenario4. 


X 

-0.0018 

-0.0024 

-0.0015 

-0.0010 

-0.0271 

-0.0442 

0.0246 

0.1971 

0.2699 

y 

125 

325 

625.02 

1025.1 

1525.2 

2125.6 

2826.4 

3628.6 

4533.1 

z 

0.0000 

0.0000 

0.0000 

0.0001 

0.0001 

0.0002 

0.0004 

0.0006 

0.0010 

Table  4.6:  Barometric  altimeter  added  object  posi¬ 
tions  for  Scenario4. 


X 

-0.0018 

-0.0024 

-0.0015 

-0.0010 

-0.0271 

-0.0442 

0.0246 

0.1971 

0.2699 

y 

125 

325 

625.02 

1025.1 

1525.2 

2125.6 

2826.4 

3628.6 

4533.1 

z 

-500 

-500 

-500 

-499.9 

-499.9 

-499.8 

-499.6 

-499.4 

-499 

Table  4.7:  True  object  positions  for  Scenario4. 


X 

-0.1388 

-0.1360 

-0.0262 

-0.0083 

-0.1158 

-0.0988 

0.0332 

0.1551 

0.1355 

y 

213.5 

330.3 

752.1 

1315.7 

2063.0 

3142.3 

4376.4 

6006.0 

8105.4 

z 

-493.5 

-490.4 

-491.2 

-494.3 

-494.1 

-499.5 

-499.2 

-490.1 

-497.5 

Kalman  Filter  Position  Error  (Est  -  True) 


0  20  40  60  80  100 


Figure  4.9:  Scenario  4.  Kalman  Filter  Position  Errors  after  Least  Square  Method 
Applied.  Estimated  -  true.  Red  is  H —  standard  deviation. 
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Figure  4.10:  Scenario  4.  Kalman  Filter  Velocity  Errors  after  Least  Square  Method 
Applied.  Estimated  -  true.  Red  is  H —  standard  deviation. 


Table  4.8:  Scenario  4.  Standard  deviation  results 

obtained  from  Kalman  filtering  after  Least  Square 
Method  Applied. 


Standard  Deviation 

Value 

24.92  (m) 

ay 

17.488  (m) 

(^Z 

24.668  (m) 

^VX 

5.6601  {m/s) 

^vy 

4.9942  {m/s) 

^vz 

1.1479  {m/s) 

(T9 

0.10047  (rad) 

^  (p 

0.19122  (rad) 

0.98697  (rad) 

63 


Table  4.9:  Standard  deviation  results  obtained  from 
Kalman  filtering  of  Scenariol,  Scenario2,  and  Sce¬ 
narios. 


Std 

Scenariol 

Scenario2 

Scenario3 

28.3415  (m) 

26.0850  (m) 

21.7347  (m) 

ay 

26.7456  (m) 

17.5229  (m) 

17.2366  (m) 

24.8587  (m) 

23.7029  (m) 

18.4061  (m) 

^VX 

5.8403  {m/s) 

5.6905  {m/s) 

5.3178  {m/s) 

^vy 

5.5674  {m/s) 

4.9955  {m/s) 

4.9784  {m/s) 

(^VZ 

1.1592  {m/s) 

1.1419  {m/s) 

1.0640  {m/s) 

(Te 

0.1031  (rad) 

0.1005  (rad) 

0.1004  (rad) 

^  (j) 

0.1916  (rad) 

0.1845  (rad) 

0.1841  (rad) 

0.9892  (rad) 

0.9216  (rad) 

0.9264  (rad) 

4-6  Analysis 

We  need  to  compare  plot  results  and  statistics  before  state  our  conclusion.  Look 
at  the  table  that  shows  statistical  results  for  hrst  three  scenarios  above.  It  is  obvious 
that  we  have  smaller  standard  deviations  when  we  have  more  tracked  objects.  If 
we  look  at  the  error  plots  for  scenarios,  we  can  see  that  the  worst  one  is  the  hrst 
scenario  where  we  have  one  object  tracked,  and  third  scenario  looks  better  than  hrst 
and  second.  Results  for  second  scenario  are  better  than  hrst.  However,  comparing 
plots  and  statistics  we  expected  to  see  impact  of  using  two  or  more  tracked  object  to 
prove  the  claim  very  clearly.  Even  that  second  scenario  has  better  results  in  comparing 
with  hrst  one,  diherence  between  them  is  not  signihcant.  There  were  unexpected  error 
grows  in  z  direction.  The  theory  explained  in  Chapter  3  gives  us  measurement  update 
for  X  and  y  positions,  and  as  investigated  in  scenario  4  we  need  to  have  barometric 
altimeter  aid.  Results  could  be  improved  by  tuning  hlter  more  accurately,  and  that 
requires  further  study  on  scenarios  and  code. 
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V.  Conclusions 


5. 1  Conclusion 

In  the  first  step  of  this  study  the  simplest  2-D  scenario  of  INS  aiding  using  bearing 
measurements  of  stationary  ground  features  is  investigated.  The  measurements  are 
taken  over  time  and  the  attendant  observability  problem  is  formulated  and  analyzed. 
The  degree  of  INS  aiding  action  is  determined  by  the  degree  of  observability  provided 
by  the  measurement  arrangement.  The  latter  is  strongly  influenced  by  the  nature  of 
the  available  measurements  -  in  our  case,  bearing  measurements  of  stationary  ground 
objects  -  the  trajectory  of  the  aircraft,  and  the  length  of  the  measurement  interval. 
Whereas  observability  guarantees  that  all  the  navigation  state’s  components  are  pos¬ 
itively  affected  by  the  external  measurements,  we  are  also  interested  in  the  possibility 
of  partial  observability  where  not  all  the  navigation  state  components’  estimates  are 
impacted  by  the  external  measurements.  It  is  shown  that  when  one  known  ground 
object  is  tracked,  the  observability  Grammian  is  rank  dehcient  and  thus  full  INS  aid¬ 
ing  action  is  not  available.  However,  if  baro  altitude  is  available  and  an  additional 
vertical  gyroscope  is  used  to  provide  an  independent  measurement  of  the  aircraft’s 
pitch  angle,  a  data  driven  estimate  of  the  complete  navigation  state  can  be  obtained. 
If  two  ground  features  are  simultaneously  tracked  the  observability  Grammian  is  full 
rank  and  all  the  components  of  the  navigation  state  vector  are  positively  impacted 
by  the  optical  measurements.  The  simulation  we  applied  here  do  not  fully  validate 
the  theory  and  there  full  further  work  is  needed. 

In  the  second  step  of  this  study  simple  scenario  for  3-D  flight  and  INS  aiding 
using  bearing  measurements  of  stationary  ground  features  is  investigated.  Ground 
objects  are  generated  and  measurements  are  obtained  using  those  ground  objects.  In 
the  scenarios,  one  ground  object  tracking,  two  ground  objects  tracking,  and  three 
ground  objects  tracking  investigated.  Having  more  ground  objects  provided  some 
better  results,  and  also  smaller  standard  deviations.  In  theory  and  in  application  we 
note  that  each  measured  position  of  the  aircraft,  which  also  refers  to  an  image  taken 
by  the  camera,  is  related  to  two  ground  object  such  that  are  common  in  two  neighbor 
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images.  In  situation  to  have  less  than  two  matched  ground  objects/features  INS  prop¬ 
agation  will  continue,  however  measurement  propagation  chain,  showed  in  Chapter 
four  Scenario  2,  will  be  crashed.  Three  ground  objects  are  necessary  to  initialize  the 
process  only.  After  that  point,  hrst  two  good  measurements  would  be  set  as  new  ini¬ 
tial  condition,  and  that  will  be  the  second  start  point  for  measurement  propagation. 
Measurements  iterations  after  that  can  be  assumed  as  good  relative  to  that  second 
start  point.  Some  other  measurement  input  or  method  would  be  essential  to  obtain 
geometric/geodetic  relation  between  last  good  measurement  of  hrst  chain  and  hrst 
good  measurement  of  last  chain. 

5. 2  Next  Iterations 

Here  we  have  some  recommendations  for  further  studies. 

•  Euler  angles  are  assumed  small  in  this  study.  One  can  extend  the  study  using 
second  terms  of  Taylor  series  of  sine  and  cosine.  After  that  some  nonlinear  methods 
or  extended  linearisation  would  be  essential. 

•  This  study  was  designed  to  be  a  part  of  control  and  navigation  system,  that 
consists  of  image  processing  block  upfront,  Kalman  Filter  for  navigation  solution, 
LQG  hight  controller,  communication  system,  weapon  systems  and  other  subsystems. 
Those  who  work  on  manual  target  selection  might  use  algorithm  in  this  study  to  ob¬ 
tain  geo-location  of  selected  target.  Then  it  is  easy  to  obtain  relative  distance  to  the 
selected  target,  so  one  can  consider  to  give  inputs  to  approach  closer  to  the  selected 
target  or  hy  away  from  it.  It  could  be  done  either  manually  or  a  simple  algorithm 
that  has  those  two  options  could  be  adopted  to  the  LQG  controller. 

•  Those,  who  do  research  about  flight  patterns  to  observe  possible  enemies,  to 
protect  moving  friendly  troopers,  could  extend  their  research  for  patterns  also  to  fly 
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over  gaps  where  measurement  propagation  chains  are  broken,  explained  at  the  end 
of  conclusions,  to  obtain  new  and  useful  measurement  information.  That  would  be 
useful  to  connect  separate  chains. 
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Appendix  A.  The  Point  of  “Inters eetion”  of  Two  Direeted  Straight 

Lines  in 


The  two  straight  lines  are 


/i  =  {P|P  =  Pi  +  m,t>0} 

/2  =  {P|P  =  P2  +  tl4,t>0} 

The  lines’  “points  of  origin”  are  Pi  and  P2,  respectively.  Vi  and  V2  are  unit 
vectors  in  P^. 

Lemmal  The  distance  d(P,  1)  from  a  specihed  point  P  to  the  straight  line 

/i  =  {P|P  =  Pi  +  m,t>0} 

is 

IIP-P1II2  tf  Vf{P-PAAt) 

VHP  -  Pilli  -  (P  -  p,Yvv'^{p  -  Pi)  if  vf{p-PA>o 


Proof:  Solve  the  optimization  problem 


mm,,3ji(Pi  -  P  +  tVf{P,  -P  +  tV) 

We  obtain 


wherupon  we  calculate  d. 


t*  =  V^{P  -  Pi) 


□ 


The  point  of  “intersection”  P  of  two  straight  lines  in  3?^  is  such  that: 


d‘^{P,  h)  +  dI{P,  I2)  min] 


We  have  evoked  the  “least  squares”  principle. 


Thus,  the  optimization  problem  is  posed. 


minp^^3  [d^(P,  h)  +  d'^{P,  I2)] 

From  Lemma  1  we  conclude: 


mmp,5j3[(P  -  P,f{P  -  Pi)  -  (P  -  Pi)'^lhV^i'^(P  -  Pi)  +  (P  -  P2V{P  -  P2)  -  (P  - 

P2fV2V,^{P-P2)] 


P  -  Pi  -  ViV,^{P  -  Pi)  +  P  -  P2  -  V2V^{P  -  P2)  =  0 


P*  =  l[h-  +  V^2Vf )]-'  ■  [Pi  +  P2  -  (Wh^rPi  +  V2V^P2)  (A.l) 

providet  that: 


V^^[I  -  fracl2{ViV^^  +  l^2^2^)]“M^i  +  ^2  -  (^i^i^Pi  +  V^2Vf  P2)]  >  2V^ P,  (A.2) 


and 


Vf  [J  -  fracl2{ViV^^  +  l^2^2^)]“M^i  +  ^2  -  (^iVi^Pi  +  V^2Vf  P2)]  >  2lf  P2  (A.3) 


The  geometry  is  illustrated  in  Figure  A.l: 
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Figure  A.l:  Crossing  lines 


From  Lemma  1  we  know 


tl  =  V^{P*  -  Pi)  and  tl  =  V^{P*  -  P2) 
wherefrom  we  calculate 


Fi  =  Pi  +  ViV;^{P*  -  Pi)  and  F2  =  P2  +  V2V^{P*  -  P2) 
Next  calculate 


X  =  \{Fi  +  F2) 


X  =  ]^[Pi  +  P2-  {ViV^Pi  +  V2V^P2)  +  {ViV^  +  V2V^)P*]  (A.4) 

From  eq  (A.l)  we  obtain 


P1  +  P2-  iViV^Pi  +  CaVf  P2)  =  2[/  -  fracUiViV^  +  V2Vj)]P*  (A.5) 


Inserting  eq  (A.5)  into  eq  (A.4)  yields 


70 


X  =  |[2/  -  +  v^vy  +  yv^  + 


X  =  p* 


We  conclude: 

The  point  of  “intersection”  of  the  straight  lines  li  and  I2  is  the  midpoint  of  a  segment 
F1F2  where  Fi  e  li,  F2  e  I2,  and  the  segment  F1F2  is  otrhogonal  to  both  /i  and  I2. 
Finally,  F1F2  =  minFieh,F2d2d{FiF2). 

This  characterization  of  the  “intersection”  P*  of  the  staright  lines  li  and  I2  yields  the 
following  alternative  algorithm  for  the  calculation  of  P*. 


Fi  =  Pi  +  ViF, 


F2  —  P2  T  ^2^2 


Fi  —  F2  —  Pi  —  P2  Viti  —  1/2^2 
Now  VyPi  -  F2)  =  0  and  V^{Fi  -  F2)  =  0 

and  this  yields  the  two  equations  in  the  unknown  ti  and  ^2 

If  (Pi  -  P2  +  Vih  -  V2t2)  =  0 

Vf  (Pi  -  P2  +  Viti  -  V2t2)  =  0 


1  -V^V2 

vyp2  -  Pi) 

v:[Vi  -1 

\  t2  j 

vyp2  -  Pi)_ 
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1  -V;^I^2 

(P2  -  Pi) 

V2^V,  -1 

V,^{P2  -  Pl)_ 

«:  =  jvT^,v;-\i  -  v^vn(p,  -  P2) 
«2  =  (X7T|Fri'"2^|/  -  V,v:[](P,  -  P,) 


p  =  p  + 

F2  =  P2  -\- 


1 

(vfy2)2-i 

1 

(v'i^y2)2-i 


ViV^[I 

V2V^[I 


V2V^]{Pi  -  P2) 
ViV^]{P2-Pi) 


p* 

p 


=  i{P  +  P  +  -  Pl//  +  V.V^PV^  - 

•^i{P  +  p  +  -  pvf  +  Pi4’-(pp’- 

A  3  X  3  matrix  inversion  is  not  required. 


ViV,^V2V,^]{Pi  -  P2)} 

-V,VmPi-P2)} 


If 

V,^[I  -  +  V2V,^)]-^[Pi  +  P2-  +  V2V^^P2)]  <  2V^Pi 

V^[I  -  \{V,V^  +  V2V^)]-^[P^  +  P2-  (KiKfPi  +  I^2^'^P2)]  <  2V,^P2 
then 
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and 


t\=tl  =  Q 


P*  =  \{Pl  +  P2) 
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The  situation  is: 


Figure  A. 2:  P*  is  the  point  in  the  middle  of  the  shortest  distance  between  li  and 

h 


If 


vl[i  -  +  V2V^)]-^[p^  +  P2 


(hdl/f  A  +  V2V^P2)]  >  21/f Pi 


but 


v:[[i  -  \{v,v^  +  V2V^)]-\p^  +  P2 


(l/lf^l^Pl  +  V2V^P2)]  <  2V^P2 
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The  situation  is: 


I2 


Figure  A. 3: 


Using  Lemma  1  we  obtain 


tl  =  Ui^(P2  -  Pi)  ^  Pi  =  Pi  +  UiUi^(P2  -  Pi) 


P*  =  i[P,  +  P2-UiUf(Pi-P2)] 


In  summary,  the  following  holds 


Theorem  2  The  point  of  “intersection”  of  the  straight  lines  li  =  {P|P 
Pi  +  tVi,  t  >  0}  and  I2  =  {P|P  =  P2  +  f  >  0}  is  calculated  as  follows. 


If  the  problem  parameters  satisfy 


V,^[I  -  |(UiUf  +  U2U/)]-i[Pi  +  P2 


(UlUf  Pi  +  U2U/P2)]  >  2Uf  Pi 


and 
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V^[I  -  +  V2V^^)]-^[P,  +  P2-  {V,V,^P,  +  V2V^P2)]  >  2Vf  P2 

The  point  of  “intersection”  of  the  straight  lines  /i  and  I2  is 


P*  =  |[/  -  +  1^2 ■  [Pi  +  P2-  {VlV,^P,  +  V2V,^P2)] 

An  alternative  formula  is: 


p* 


|{-Pi  +  P2  + 


f/2Vf  +  V,V^{V2V^ 


v,vmpi-P2)} 


If  the  parameters  are  such  that: 


V,^[I  -  +  V2V,^)]-^[P,  +  P2 


Pi  +  V2V^P2)]  <  2Vf  Pi 


and 


V2^[I  -  UViVi^  +  V2V2^)]~"[Pi  +  P2-  (IhV^fPl  +  f"2^'^P2)]  <  2Vf  P2 
then 


P*  =  UPi  +  P2) 

If  the  problem  parameters  are  such  that: 


v,^[i  -  + ^2^^r'[Pi + P2 


(IhV^f  Pi  +  f^2^'^P2)]  >  2Vf  Pi 


and 


+  ^2Vf  )]-MPl  +  P2  -  (VdV^fPl  +  1^2 ^'^P2)]  <  2Vf  P2 

Then 


P*  =  1  [p^  +  P2  -  ViV,^{Pi  -  P2)] 
If  the  problem  parameters  are  such  that: 


l"l^[/  -  +  W2^)]-MPi  +  P2 


(IhV^f  Pi  +  1^2 ^'^P2)]  <  2Vf  Pi 


and 
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V^[I  -  +  V2V^)]-^[P^  +  P2-  {V,V,^P,  +  V2V^P2)]  >  2V^P2 

then 


p*  =  l[p^  +  p^-V2V^{Pl-P2) 


□ 
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Appendix  B.  Plots  for  Scenario  1 


Measurement  Positions  of  UAV  and  Ground  Objects 


Figure  B.l:  Scenario  1.  Simulated  positions  of  the  UAV  and  ground  objects. 


78 


True  vs.  Calculated  Positon  Estimates 
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Figure  B.2:  Scenario  1.  True  vs  calculated  position. 


Figure  B.3: 


Scenario  1.  True  vs  calculated  velocity. 
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Tme  vs.  Calculated  Acceleration  Estimates 
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Figure  B.4:  Scenario  1.  True  vs  calculated  acceleration. 


Position  Errors  (Calc  -  True) 
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Figure  B.5:  Scenario  1.  Position  errors. 
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Velocity  Errors  (Calc  -  True) 


Figure  B.6:  Scenario  1.  Velocity  errors. 


Acceleration  Errors  (Calc  -  True) 
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Figure  B.7:  Scenario  1.  Acceleration  errors. 
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True  vs.  Estimated  Position  Params 


Time  (sec) 


Figure  B.8:  Scenario  1.  Kalman  Filter  True  vs  Estimated  Position  Parameters. 

Red  is  H —  standart  deviation.  Blue  -  true,  green  -estimated. 


True  vs.  Estimated  Velocity  Params 
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Figure  B. 9:  Scenario  1.  Kalman  Filter  True  vs  estimated  Velocity  Parameters.  Red 

is  H —  standart  deviation.  Blue  -  true,  green  -estimated. 
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500 


Kalman  Filter  Position  Error  (Est  -  True) 


Figure  B.IO:  Scenario  1.  Kalman  Filter  Position  Errorts.  Estimated  -  true.  Red  is 
H —  standart  deviation. 
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Figure  B.ll:  Scenario  1.  Kalman  Filter  Velocity  Errorts.  Estimated  -  true.  Red  is 
H —  standart  deviation. 
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Appendix  C.  Plots  for  Scenario  2 


Measurement  Positions  of  UAV  and  Ground  Objects 


Figure  C.l:  Scenario  2.  Simulated  positions  of  the  UAV  and  ground  objects. 
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True  vs.  Calculated  Positon  Estimates 
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Figure  C.2:  Scenario  2.  True  vs  calculated  position. 


True  vs.  Calculated  Velocity  Estimates 


Figure  C.3: 


Scenario  2.  True  vs  calculated  velocity. 
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Tme  vs.  Calculated  Acceleration  Estimates 


Figure  C.4:  Scenario  2.  True  vs  calculated  acceleration. 
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Figure  C.5: 


Scenario  2.  Position  errors. 


Velocity  Errors  (Calc  -  True) 
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Figure  C.6:  Scenario  2.  Velocity  errors. 
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Figure  C.7:  Scenario  2.  Acceleration  errors. 
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True  vs.  Estimated  Position  Params 
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Figure  C.8:  Scenario  2.  Kalman  Filter  True  vs  Estimated  Position  Parameters. 

Red  is  H —  standart  deviation.  Blue  -  true,  green  -estimated. 
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Figure  C.9:  Scenario  2.  Kalman  Filter  True  vs  estimated  Velocity  Parameters.  Red 
is  H —  standart  deviation.  Blue  -  true,  green  -estimated. 


Figure  C.IO:  Scenario  2.  Kalman  Filter  Position  Errorts.  Estimated  -  true.  Red  is 
H —  standart  deviation. 
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Figure  C.ll:  Scenario  2.  Kalman  Filter  Velocity  Errorts.  Estimated  -  true.  Red  is 
H —  standart  deviation. 


Appendix  D.  Plots  for  Scenario  3 


Measurement  Positions  of  UAV  and  Ground  Objects 


Figure  D.l:  Scenario  3.  Simulated  positions  of  the  UAV  and  ground  objects. 
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True  vs.  Calculated  Positon  Estimates 


Figure  D.2:  Scenario  3.  True  vs  calculated  position. 


True  vs.  Calculated  Velocity  Estimates 


Figure  D.3: 


Scenario  3.  True  vs  calculated  velocity. 
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Tme  vs.  Calculated  Acceleration  Estimates 


Figure  D.4:  Scenario  3.  True  vs  calculated  acceleration. 
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Figure  D.5:  Scenario  3.  Position  errors. 
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Velocity  Errors  (Calc  -  True) 


0  20  40  60  80  100 


Figure  D.6:  Scenario  3.  Velocity  errors. 


Acceleration  Errors  (Calc  -  True) 
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Figure  D.7:  Scenario  3.  Acceleration  errors. 
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True  vs.  Estimated  Position  Params 


Figure  D.8:  Scenario  3.  Kalman  Filter  True  vs  Estimated  Position  Parameters. 

Red  is  H —  standart  deviation.  Blue  -  true,  green  -estimated. 
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Figure  D.9:  Scenario  3.  Kalman  Filter  True  vs  estimated  Velocity  Parameters.  Red 
is  H —  standart  deviation.  Blue  -  true,  green  -estimated. 
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Kalman  Filter  Position  Error  (Est  -  True) 


Figure  D.IO:  Scenario  3.  Kalman  Filter  Position  Errorts.  Estimated  -  true.  Red  is 
H —  standart  deviation. 


Kalman  Filter  Velocity  Error  {Est  -  True) 


Figure  D.ll:  Scenario  3.  Kalman  Filter  Velocity  Errorts.  Estimated  -  true.  Red  is 
H —  standart  deviation. 


95 


Appendix  E.  Plots  for  Scenario  4 


Measurement  Positions  of  UAV  and  Ground  Objects 


Figure  E.l:  Scenario  4.  Simulated  positions  of  the  UAV  and  ground  objects. 
E.0.1  True  and  Calculated  Trajectory  and  Velocity  Plots. 
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True  vs.  Calculated  Positon  Estimates 
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Figure  E.2:  Scenario  4.  True  vs  calculated  position. 


True  vs.  Calculated  Velocity  Estimates 
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Figure  E.3:  Scenario  4.  True  vs  calculated  velocity. 
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Tme  vs.  Calculated  Acceleration  Estimates 


Figure  E.4:  Scenario  4.  True  vs  calculated  acceleration. 
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Figure  E.5:  Scenario  4.  Position  errors. 
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Figure  E.6: 


Scenario  4.  Velocity  errors. 


Acceleration  Errors  (Calc  -  True) 


Figure  E.7:  Scenario  4.  Acceleration  errors. 
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True  vs.  Estimated  Position  Params 
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Figure  E.8:  Scenario  4.  Kalman  Filter  True  vs  Estimated  Position  Parameters. 

Red  is  H —  standart  deviation.  Blue  -  true,  green  -estimated. 


True  vs.  Estimated  Velocity  Params 


Time  (sec) 


Figure  E.9:  Scenario  4.  Kalman  Filter  True  vs  estimated  Velocity  Parameters.  Red 
is  -I —  standart  deviation.  Blue  -  true,  green  -estimated. 

E.0.2  Kalman  Filter  Results  from  Perfect  Measurement. 
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Figure  E.IO:  Scenario  4.  Kalman  Filter  Position  Errorts.  Estimated  -  true.  Red  is 
H —  standart  deviation. 
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Figure  E.ll:  Scenario  4.  Kalman  Filter  Velocity  Errorts.  Estimated  -  true.  Red  is 
H —  standart  deviation. 
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True  vs.  Estimated  Position  Params 
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Figure  E.12:  Scenario  4.  Kalman  Filter  True  vs  Estimated  Position  Parameters 

after  Least  Square  Method  Applied.  Red  is  H —  standart  deviation.  Blue  -  true, 
green  -estimated. 

E.  0.3  Kalman  Filter  Results  after  Least  Square  Method  Applied  for  Object  Geo- 
Location. 
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Figure  E.13:  Scenario  4.  Kalman  Filter  True  vs  estimated  Velocity  Parameters 

after  Least  Square  Method  Applied.  Red  is  H —  standart  deviation.  Blue  -  true, 
green  -estimated. 


Kalman  Filter  Position  Error  {Est  -  True} 


Figure  E.14:  Scenario  4.  Kalman  Filter  Position  Errors  after  Least  Square  Method 
Applied.  Estimated  -  true.  Red  is  -f—  standart  deviation. 
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Kalman  Filter  Velocity  Error  (Est  -  True) 


-500' - ■ - ■ - ■ - ■ - 1 

0  20  40  60  80  100 

Time  (sec) 


Figure  E.15:  Scenario  4.  Kalman  Filter  Velocity  Errors  after  Least  Square  Method 
Applied.  Estimated  -  true.  Red  is  H —  standart  deviation. 
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